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ABSTRACT 


An autonomous vehicle must be able to determine its global position even in the absence 
of external information input. To obtain reliable position information, this would require the in- 
tegration of multiple navigation sensors and the optimal fusion of the navigation data provided by 
them. 

The approach taken in this thesis was to implement two navigation sensors for a four-wheel 
drive and steer autonomous vehicle: An inertial measurement unit providing linear acceleration in 
three dimensions and angular velocity for the vehicle’s global motion and shaft encoders providing 
local motion parameters. An inertial measurement unit is integrated with the Shepherd mobile 
robot and data acquisition and processing software is developed. Position estimation based on shaft 
encoder readings is implemented. The framework for future analysis including most general motion 
profiles have been laid. 

The sensor’s system performance was evaluated using three different linear motion profiles. 
Test results indicate that the shaft encoder provide a positioning accuracy better than 99% (typ. 7.5 
mm for 1 m motion) under no slip conditions for pure translational motion. The IMU still requires 


further improvement to allow for both sensors to be combined to an integrated system. 
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[. INTRODUCTION 


A. BACKGROUND AND MOTIVATION 


Landmines have become an ever increasing threat for the civilian communities in post-war 
scenarios. Several million land mines are scattered around the world annually causing more than 
10,000 fatalities and more than 20,000 severe injuries to non-combattants. 

Since effective multi-national proliferation treaties banning the use of anti-personnel mines 
are not yet in place and with major producers for those mines not likely to sign these treaties because 
of their important impact on conventional warfare, it is essential to develop and deploy equipment 
for detection of anti-personnel mines in mine-contaminated regions. 

Moreover, many countries are downsizing their armed forces due to budget constraints and 
thus turning over formerly used defense sites to the local communities. Wide areas of these defense 
sites (such as proving ground, rifle ranges, ...) are contaminated with unexploded ordnance (UXO). 


The contaminated land must be cleared before transferring to civilian use. 


B. OBJECTIVE 


At present, there are not many effective means for mine and UXO detection available. 
The current approach to mine and UXO detection and clearance is labor and time intensive and 
dangerous: explosive ordnance disposal (EOD) personnel walks slowly over the area that is to be 
cleared, trying to detect buried, half buried or totally exposed material. Once an object is found, 


successive steps in the clearance process would include: 


e detect, 

e identify, 

e excavate, 

e defuse, 

e transport 
and 


e dispose 


the object in question. It is therefore desireable to develop a robust, low-cost tool for persuing 
the above steps through the use of robotics and advanced sensing techniques meeting the following 


requirements: 


e Robustness for operation in rough terrain 
e Expandability for different sensors and equipment 


e Precise navigation tools 


Multi-disciplinary research conducted in the Departments of Electrical and Computer Engi- 
neering, Computer Science, Aeronautics and Astronautics, and the Physics Department at the Naval 
Postgraduate School, centers around the development of a semi-autonomous robot system for land 
mine/UXO searching/processing tasks in humanitarian operations [2]. This project has required the 
cooperative effort of several NPS thesis. The emphasis of this thesis is the implementation of an 
integrated navigation system. In the long term, the system components will be comprised by a land 
vehicle, an aerial vehicle, and a ground-based control center. 

The land vehicle, specifically designed for the aforementioned tasks is four-wheel steerable 
and drivable. A prototype vehicle called SHEPHERD is currently in use for this research project. 
The unique design of SHEPHERD provides a high level of sophistication for motion control for it 
to be able to precisely traverse rough terrain. The interested reader is referred to [1]. The scope of 
this project, in general, is very comprehensive and encompasses many scientific areas. In particular, 
interdisciplinary tools such as physics principles including coordinate transformations, kinematics 
and mechanics of rigid bodies, and electrical and software engineering tools are used, discussed and 
covered in this thesis. 

In order to control the vehicle and implement efficient search patterns while at the same time 
reducing redundant search paths, precise knowledge of the vehicle’s velocity and position is essential. 
Using an on-board inertial navigation system, the vehicle’s acceleration can be measured and it’s 3D 
motion precisely computed by the on-board computer. However, an inertial sensor alone can provide 
accurate position information only in the short term, but must be integrated with additional sensors 
if precise long term positional data is required. The vehicle’s rough operation environment makes 
it essential that extremeley accurate position information is obtained. To meet this requirement, a 
Global Positioning System (GPS) receiver shall be integrated. 

The purpose of this thesis is to implement and evaluate an integrated navigation system for 
SHEPHERD enabling the operation of the vehicle under extremely rough conditions while at the 
same time providing accurate position information. This thesis will examine the following research 


questions: 


1. Provide the theoretical background for coordinate transformations, 
2. Implement the hardware and software for an Inertial Measurement Unit (IMU), 


3. Implement the software to determine position based on the on-board shaft encoders, 


4. Develop a scheme for sensor fusion for slip-detection. 


Cc. ORGANIZATION 


First, a brief overview of the computer architecture for the Shepherd Rotary Vehicle is given 
in Chapter II. Secondly, Chapter III defines the basic reference frames that are being used throughout 
this project. The secondary means of determining the vehicle motion is given by shaft encoders 
that are used for each of four wheels for both, steering and driving. The software implementation is 
described in Chapter IV. Chapter V describes the implementation of a low cost inertial measurement 
system (IMS) both in hardware and software. Its purpose will be to complement the shaft encoder 
system in situations were slip occurs. How both systems may be unified for slip detection and to 
further improve the performance of the navigation system is investigated in Chapter VI. Finally, 
the success and limitations of the use of the system described herein is summarized in Chapter VII 


providing essential results of this research and recommendations for future research in this area. 





Il. SYSTEM OVERVIEW 


In this chapter we will give a brief computer hardware description of the system configuration 
for the SHEPHERD Rotary Vehicle. This complements the description given by Mays/Reid [1] and 
is intended to provide the essential information necessary to understand the cross-references to 
computer components given in the following Chapters. 

All mechanical information for the mobile platform is extensively discussed by Mays/Reid 
[1]. However, we shall note at this point that the Shepherd Rotary Vehicle is a four wheel drive 
and steer mobile robot. The four wheels are steerable without limitations and can be rotated and 
driven in either direction (more than 360 degree of rotation space). The four wheel drive and 
steer capability shall provide the robustness required for operation in rough terrain (e.g., sand dune 
scenarios, ...). A side view and front view photo taken fom SHEPHERD with a digital camera are 
shown in Figure 2.1 and Figure 2.2, respectively. 

In Figure 2.1 we can can see the four suspension boxes for the four wheels, the steel plate 
that comprises the main support frame for the robot, the inertial measurement sensor mounted 
upside down below the steel plate, and a joystick that is used to manually steer the robot in the 
top right-hand corner. In addition, in the rear view photo you can see the Laptop computer, to its 
left a switchbox for connecting the Laptop to either a CONSOLE or HOST serial port, and to its 
right the joystick. Another view, shown in Figure 2.3 shows the Laptop placed on the steel plate 
and behind it the servo control rack and the VMEBus chassis. 

The complete hardware architecture is comprised of the TAURUS Single Board Computer 
[3], a VME-Bus based single board computer with a Motorola MC68040 as main processor and 
several other on-board processing components and the VME-Bus. At present, this stand alone 
computer system is expanded with a servo controller unit that interfaces to the four wheels and 
a 16-channel differential input A/D-Board. Four channels of the A/D-Board are utilized for the 
inertial measurement unit (IMU) which is discussed in Chapter V. In the future, the system may 
be expanded with several other sensors through the use of the VME-Bus. Figure 2.4 shows a block 
diagram of the system configuration for SHEPHERD. 


A. TAURUS BOARD 


This section describes the TAURUS single-board computer system’s main features. The 


hardware is based on a dual processor platform using Motorola’s 68040 as the main processor and 





Figure 2.2: Front view from the SHEPHERD Rotary Vehicle with wheels rotated by 45° 





Figure 2.3: Top view from the SHEPHERD Rotary Vehicle. In the front, the Pentium Laptop used 
as a concole, in the middle the servo controller chassis, and in the back the VMEBus rack. 
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Figure 2.4: Shepherd Rotary Vehicle Hardware Configuration. 


the 68030 as a slave processor for basic I/O functions. Signaling between both processors takes place 
via processor interrupts. The system is attached to a VME bus backplane providing the capability 
to expand the system as far as main memory and additional sensor devices are concerned. Among 


the many I/O functions that the TUARUS board provides are: 


e six RS-232C serial communication ports (two through a DUART 68C681, and four through 
a CD2401 Communications Device) 


e two 24 bit parallel ports 


e several timer/counters: Five provided by the AM9513A System Timing Controller, one 
timer is provided in the 68C681 serial port device and eight timer/counters are available in 
the CD2401 


e real time calendar clock device MK48T08 
e SCSI Port 


e Ethernet Port 


More information can be obtained from [3] and the respective operating/user manuals for 
each device. Rather than focussing on all the technological aspects for each device, we merely focus 


on those important ones for understanding the operation of SHEPHERD. 


iP TAURUS Bug Monitor/Debugger 


For start-up and debugging/monitoring purposes, a debugger/monitoring program called 
TAURUSBug resides in the memory region from 0xff800000 through Oxff9fffff (memory bank 
2, see [3], Chapter 2.2). The user may decide whether or not to use this program for the boot-up. 
However, in the sequel, the project group has made heavy use of the debugging tools provided by 
TAURUSBug. 


Za. DUART 68C681 


The TAURUS board features a 68C681 device which provides two dual asynchronous re- 
ceiver /transmitter (DUART) serial ports with RS-232C interface. These two ports are utilized for 
up-/and downloading of executable code and data and for user interaction with SHEPHERD. Port 
A is called CONSOLE and Port B is called HOST. Both ports are connected through a switchbox 
to the laptop computer. 


3. Cirrus Logic Communications Controller CD2401 


Up to date, only one of the four RS-232C serial ports provided by the Cirrus Logic Com- 


munications Controller CD2401 is used for interfacing the GPS receiver. 


A. AM9513A Counter/Timer 


The AM9513A LSI circuit provides a total of five independent 16-bit timer/counters which 
can be cascaded to a single 80-Bit timer /counter for long-term observations. The timer number five 
is used for deriving the motion control clock of T=10 ms: every 10 ms a timer interrupt is issued to 
trigger another motion control cycle. This 10 ms timer interrupt is clearly the heart of the system. 
Care should be taken that this interrupt is granted the highest priority level available. This leads 


to the decision to use timer five instead one of the other four. 


5. Programmable Parallel I/O Port Device (Intel 82C55<A) 


The Taurus board is equipped with two Intel 82C55A devices each providing three 8-Bit wide 
ports (Port A, B, and C). Only the first device is currently in use for the motion control by means 
of a joystick. A simple PCB board interfaces an IBM-PC Joystick to this I/O device. However, 
some minor changes to the layout of the Joystick circuitry had to be made. Port A comprises the 
x-Position (an 8-bit digital value ranging from 0 ... 255 equivalent to joystick left to right), Port B 
gives the y-Position in the range 0 ... 255 equivalent to down (0x00) and up (Oxff). Currently, only 
Bits zero and one are in use from Port C providing status information for the two switches on the 
throttle (pushing the left switch or the center switch on the trottle will set bit zero and pressing the 
right button on the throttle will set bit one). The other two push buttons on the left-hand side of 
the joystick have currently no function. In case that needed, they can easily be connected to any of 


the six remaining bits of Port C through the PCB board by use of pull-up resistors. 


6. Interrupts 


Both on-board and off-board Interrupts are supported by the TAURUS board. All on-board 
Interrupts are routed through the Interrupt Steering Mechanism (ISM) to either the 68030 I/O 


10 


Processor or via a VMEbus Interface Controller device (VIC068) to the 68040 Processor. Note that 
an interrupt can only be routed to one processor at a time. The VIC068 guides both, ISM interrupts 
and VMEbus interrupts to the 68040 processor. This is depicted by Figure 2.5. In accordance with 
[3], the local interrupts by on-board sources from the ISM to the VIC will be labelled as LIRQ-x 
whereas the interrupts form the VIC068 to the 68040 processor are labelled IRQ-x. 


VMEbus Interrupts 


VIC068 
















Interrupts ISM 
from Interrupt 
On-Board steering 
Devices mechanism 


Figure 2.5: Servicing of on-board Interrupts or off-board VME-Bus Interrupts (From Ref. [3]) 


The ISM combines groups of on-board Interrupts to act as a single interrupt source towards 
either the 68030 or 68040 processor. It is important to note that the VIC068 device enables the 
programmer to shift the interrupt levels. In order to handle the proper handshaking in this case, 
the appropriate LIRQ-Shift-Register in the ISM would have to be set. The TAURUS user’s manual 
[3] p. 2-71 gives the following example: 

.. if LIRQ-5 from the ISM is shifted in the VIC068 to IRQ-3, then the acknowledge signal 
from the 68040 processor to the VICO68 would be IACK-3 which would be passed on to the 


ISM device. LIRQ-SR5 (at $FFF4800A - upper nibble) would be set to shift [the] VICO68 
IACK-3 input to output ISM-IACK-5. 


Some facts that should be remembered: 


e each Interrupt group is associated with an ISM Configuration Register Nibble. 
e the MSB of each Nibble is the steering Bit, where ‘0’ routes the interrupt to the 68030. 
e the remaining three bits of each nibble encode the local interrupt level. 


e upon Power-Up or RESET, all On-Board Interrupts are disabled. 


Taurus Vector Table Base address is at Oxffe10xxx where xxx = 4 x Vector Number. 
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Bs MOTION CONTROL 


As indicated in the previous section, a motion control cycle is initiated with every 10 ms 
timer interrupt. In brief, this motion control cycle is given by the following sequence of logical 


blocks: 


readEncoder() Read all shaft encoders 

computeRates() Compute (angular) velocity for all steering and driving motors 

bodyMotion() Compute motion parameters for the vehicle’s body (bodyMotion) 

wheelMotion() Compute the angles and speeds required for each wheel based on 
the results of bodyMotion 

driveMotors() | Update the servos for driving and steering motors 


The organization of the motion control cycle is described in more detail in Mays/Reid [1]. 
However, it should be noted that the source code as given there has been modified slightly to make 


the routines more efficient and thus less time consuming. 


7, 


IIT. REFERENCE FRAMES 


This chapter gives a brief discussion on reference frames that are being used throughout 


this thesis. 


A. BODY MOTION 


In the analysis of the motion of a rigid body, it turns out that considerable simplification 
in the mathematical formulas for rigid-body motion can be reached if the motion is described with 
respect to its principal axes. The principal axes are chosen such that the cross terms (sometimes 
called the products of inertia) of the moment of inertia tensor J vanish (see [4] for a more detailed 
analysis of this). The axes form a right-handed coordinate system with the origin usually taken to 
be at the body’s center of mass (CM). However, at this point we are not concerned with the moment 


of inertia tensor. 


1. Body Reference Frame 


For the purpose of describing the kinematics of a body moving on the Earth’s surface the 
reference frame is chosen such that axes of the body frame, which we will call frame {B}, are given 
by the principal axes of the body given as follows: 

x - longitudinal axis (oriented from rear to front of body) 


y - transversal axis (oriented to the left) 
z - normal axis (oriented pointing up, away from the center of the Earth) 


2 Sensor Reference Frame 


Sensors will be employed with a vehicle in order to measure parameters pertaining to the 
vehicle’s kinematics. The sensor will provide data relative to its own frame, which we will call sensor 
frame {S}. In general, this frame can be completely different from the body frame. If sensing data 
is provided in a Cartesian coordinate system, the only difference between {B} and {S} might be an 


offset (or translational difference) ?P 5 org. 
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a Earth Reference Frame 


In order to express the motion of a body as observed by an outside inertial observer we need 
to define a suitable inertial reference frame. An inertial reference frame is defined to be the frame 
for which Newton’s laws of motion are valid. For a slow moving vehicle at a particular point on the 
Earth’s surface, a suitable reference frame {R} is set up in the following way: 

xX - pointing north 

y - pointing east 

z - pointing down, towards the center of the Earth 
We will see later in this chapter that the axes x,y and z of this coordinate system refer to the geodetic 
descriptions of latitude, longitude and geodetic height respectively. Since we do not anticipate any 
large scale motion ( on the order of kilometers ) it is sufficient not to concern ourselves with the 


irregular shape of the Earth and with the resulting mapping/projection problems. 


B. GPS SYSTEM 


In order to describe both the GPS Satellite motion and receiver motion, it is necessary to 
choose a common reference system. Most commonly, the motion is described in terms of velocity 
and position as measured in a Cartesian Coordinate System. The most applicable coordinate system 
for GPS systems are given as follows: Satellite and GPS receiver motion are described in terms of 
the Earth-Centered Inertial and Earth-Centered Earth-Fixed coordinate systems respectively. The 


systems in use are described in detail by Kaplan [5] and are briefly explained below: 


1. Earth-Centered Inertial (ECI) Coordinate System 


In this system, the origin is the center of mass of the Earth. Satellites orbiting the Earth 
obey Newton’s second law of motion as described in this System. In the ECI system, the xy-plane 
coincides with the Earth’s equatorial plane, the +x-axis points towards some fixed point in space 
(celestial sphere), the z-axis is taken to be normal to the xy-plane pointing towards the north pole. 
The set of axis forms a right-handed coordinate system. However, due to the Earth’s inhomogeneous 
shape, irregularities in the Earth’s motion cause the ECI frame not to be truly inertial. Therefore, 
the GPS system defines the ECI reference frame as given by the constellation at 1200 hr UTC on 
January 1, 2000. 
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2. Earth-Centered Earth-Fixed (ECEF) Coordinate System 


For computing the receivers position, it is more convienient to use a system that is stationary 
in the earth frame. It is known as Earth-Centered Earth-Fixed (ECEF). As with the ECI frame, 
the xy-plane is coincident with the Earth’s equatorial plane, the x-axis points in the direction of 0° 
longitude, the y-axis points in the direction of 90° longitude. The x- and y-axes therefore no longer 


describe fixed directions in inertial space. The z-axis completes the right-handed coordinate system. 


3. Conversion between ECI and ECEF 


Conversions between ECI and ECEF system are accomplished by means of matrix trans- 
formations (rotator matrices) which are not further described in this thesis. It is assumed that the 


Satellite ephemeris data is already translated into ECEF system. 


4. World Geodetic System (WGS-84) 


The Department of Defense invented a system to model all irregularities pertaining to de- 
scribing the Earth’s gravitational motion. This system is known as the World Geodetic System 
(WGS-84). In addition to modeling the gravitational irregularities, the World Geodetic System 
provides an ellipsoidal model of the Earth. The ECEF coordinate system is affixed to the World 
Geodetic System reference ellipsoid and thus, latitude, longitude and height of a receiver can be 


specified with respect to this ellipsoid. 


C. TRANSFORMATIONS 


To define and manipulate physical quantities such as acceleration, velocity and position we 
must define coordinate systems and find transformations for describing vectors given in one system 
with respect to the other. These transformations will be accompanied by conventions for their 
representation. 

A great variety of similar transformations can be found in many textbooks. Not all of 
them are concisely formulated. It is thus rather confusing to relate different conventions given in 


different textbooks with each other; even though they may describe the same transformation. A 
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good introduction on spatial descriptions and transformations is given by [6] and we will therefore 
briefly outline the most important aspects and conventions as they pertain to our problem. 

The inertial reference frame {R} is given by the set of coordinate axis {x,y,z} where the 
xy-plane is the plane parallel to the WGS-84 reference ellipsoid (that is, the earth’s surface) with x 
pointing north, y pointing east and z pointing towards the geodetic center of the Earth. The frame 
{B} which is attached to the body is given by the set of axes { x’, y’,z’ } with 2’ pointing forward, 
y’ pointing to the left of the body and z’ completing the right-handed coordinate system. Figure 


3.1 shows both frames. 





Figure 3.1: Coordinate Frame for Body relative to point on Earth surface. The x/y-plane spans the 


plane tangent to the Earth’s surface. 


There are two governing basic methods of representing the orientation of a body (with the 
Frame {B} attached to it) with respect to the reference frame {R}. One way is to express the 
principal directions of {B} (unit vectors 2’, y’,z’) in terms of the coordinate system {R} and stack 


these three unit vectors together as the columns of a 3 x 3 proper orthonormal rotation matrix 
a A — [ay z'| 
where § R has the properties that its columns are mutually orthogonal and have unit length and 


det(% R) = 1. Moreover, it can be shown that the inverse of 2 R is simply its transpose: 


MR oS 2 (3.1) 


16 


and thus giving rise to 


-1 He 
SRER=SRERT =! 
Any vector P given with respect to {B} can then be expressed in terms of {R} by the transformation 
pid saat san 2d 


Since dealing with 3 x 3 matrices for describing orientations is usually very tedious, a second way 
of describing the orientation of a body can be derived from a result from linear algebra. Cayley’s 
formula for orthonormal matrices (cited by Craig [6]) states that any 3 x 3 orthonormal matrix can 
be specified by just three parameters. 

There are many ways to represent orientations with only three parameters. Not all of them 
are convenient and the reader may be easily confused while looking for those in different textbooks. 


In the discussion here we follow the conversion of Ref. [6]. 


1. Roll, Pitch, and Yaw 


One way of describing the orientation of a frame {B} relative to the reference frame {R} 
is by describing the body’s orientation by observing successive rotations about the three axes (x,y, 


and z) of the fixed refernece frame {R}. Craig [6] refers to this convention as X-Y-Z fixed angles: 


1. start with the frame {B} coincident with the reference frame {R} 
2. rotate {B} about "x by the roll angle 0 
3. rotate {B} about *"y by the pitch angle ¢ 
4. rotate {B} about "Z by the yaw angle w 
Each of the three rotations takes place about an axis in the fixed reference frame {R}. The resulting 


rotation matrix can be obtained by successively rotating the frame {B} about single axes in the 


stationary frame {R}: 


R se R R R 
sR = *Rz(p) *Ry(¢) “Rx(@) (3.2) 
cos(¥)cos(¢) cos(wW)sin(d)sin(d) — sin(w)cos(@) cos(y)sin(d)cos(@) + sin(y)sin(@) 
= sin(wW)cos(¢) sin(y)sin(d)sin(@) + cos(y)cos(@) sin(w)sin(¢?)cos(@) — cos(y)sin(6) 
—sin(d) cos(P)sin(@) cos(¢)cos(8@) 
where 
1 0 0 
FRx (8) = a) cos(@) —sin(@) (3.3) 
0 gin(@) cos(6) 
i cos(¢) 0 sin(¢) 
Ry(@) = | oe 1 (3.4) 
—sin(¢d) 0 co3s(¢) 


1G 


coa(y) —sin(w) 0 


“Raly) = sin(w) — coa(w) 0 (3.5) 
1 


0 0 


Therefore, a vector *a given in frame {B} can be transformed with respect to frame {R} by the 
transformation 


wot oO 


2. Euler Angles 


Another possible description of the frame {B} with respect to frame {R} is given by the 
Euler Angles. As opposed to rotating the frame {B} in successive steps about the fixed axes 
of {R}, this description will involve successive rotations performed about the principal axes of the 


rotating frame {B} we are about to move: 


1. start with the frame {B} coincident with the reference frame {R} 
2. rotate {B} about ®z by the angle w 
3. rotate {B} about *y by the angle ¢ 


4. rotate {B} about ®X by the angle 0 


The resulting rotation matrix is the same as given above in Equation 3.2. Instead of naming the. 
angles 0, ¢, ~ as roll, pitch, and yaw respectively, they are now being referred to as the Euler Angles. 
Craig refers to them as the Z-Y-X Euler Angles. This transformation is equivalent to the one 
given by Fossen [7] on page 10 except that we exchanged the naming for roll and pitch (0 + @). 
The result obtained yields a fundamental statement as given by Craig [6]: 


... three rotations taken about fixed axis yield the same final orientation as the same three 
rotations taken in opposite order about the axes of the moving frame. 


In this work, we will make reference to the Eulerian angles and this mostly to the fact that 
the Eulerian angles are easier to recognize. However, the euler angles are equivalent to the roll, yaw 
and pitch angles. 

In this chapter we have laid the framework for transforming vectors from one coordinate 
system to the other. We will apply this to the Inertial Measurement Unit and develop a scheme 


for determining the specific acceleration acting on a body even in the presence of the gravitational 


acceleration. 
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IV. POSITION DETERMINATION WITH SHAFT 
ENCODER 


This chapter describes the use of the shaft encoders for position determination. It comple- 
ments and in some cases alters the results obtained by Mays/Reid [1]. As outlined in Mays/Reid [1], 
each servo motor is equipped with shaft encoders which record the actual angles for all eight motors. 
This should provide an easy means for direct position determination under the condition that no 
slip occurs. That is, the difference between an interval T=10 ms by which each encoder (driving and 
steering) advances is directly proportional to the distance travelled or to the angle each wheel was 
rotated and accordingly for the time of observation proportional to the linear and angular velocity. 

It should be noted that the shaft encoders for the driving motors count positive for a 
clockwise rotation of the wheel. Thus, if all wheels are driving forward (which implies that wheels 1 
and 3 are commanded with negative servo data) the shaft encoder readings will decrease for wheels 
2 and 4. In the same manner, if all wheels are steering to the right (clockwise as viewed from above, 


with negative servo data commanded), the shaft encoder readings will increase for all wheels. 


A. DETERMINING THE SERVO PARAMETERS 


It might be necessary from time to time to verify and adjust the servo parameters in use 
for the motion control of SHEPHERD. Therefore, a few test routines have been implemented in the 


file ‘motor.c’. These functions are 


driveTest () to determine the driving parameters 

steerTest () to determine the steering parameters 

stopTest () to determine the interaction between driving and steering for dig- 
its commanded to the servos being zero 

velocityTest () to obtain a relationship between digits commanded to the driving 


motors and actual angle rates observed 
circumferenceTest() to determine the circumference of the wheels 


ils Steer Parameters 


For determining the steering parameters the following method has been impemented in 


function ‘steerTest()’ in file ‘motor.c’: 


19 


1. align all wheels with hall sensor 
. Clear the counters 
. save counter data in variable previous 


. rotate wheels for a certain number of turns and stop time it takes to rotate the wheel 


oo -e Wo bd 


. read shaft encoder ’current’ and compute the counter difference to obtain the rate of turn 
and number of counts for a turn 


The source code is implemented as function ‘steerTest()’ in the file ‘motor.c’. It should be 
noted that this test should only be conducted for free wheels off the ground, otherwise the vehicle 
may just wander around. 

Some characteristic data corresponding to a specific velocity commanded is shown in Ta- 
ble 4.1. It can be seen from the Table that when steering the wheel, this would interfere with the 
drive counters as well. The work of Mays/Reid account for this fact by closed loop control. The 
data was taken for no load applied to the wheels (free turning wheels). 


Wheel 1 Wheel 2 Wheel3 Wheel 4 


count per turn -92160.2 -92131.7 -92160.3 -92160.1 
counts per degree -256.00 -255.92 -256.00 -256.00 


time per turn (sec) 6.97 6.98 6.98 6.98 
drive count for turn 2048.0 2047.9 2048.0 2047.9 





Table 4.1: Steering Wheel Data at Digits commanded 0x0b00 averaged over 10 turns. 


Note when a positive value is commanded to all steering motors that the motion of the 
wheels as viewed from above is counterclockwise and the shaft encoder readings are negative! From 
the data, we can derive a relationship between the angular position of the steering motors and the 


encoder readings 





Table 4.2: Conversion Factor for Steering all Wheels. 


The results given above are in agreement with the findings from Mays/Reid [1]. With this 
data in mind, the angular velocity can be easily measured. All that needs to be done is to record 
the difference in steer encoder readings for an observation timeframe (T=10ms) and multiply by the 


above factor and divide by T. 
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Dis Drive Parameters 


What is the goal to be determined in this section is: how does the driving data commanded 
to the drive servos (in the range from -1024 to +1023) relate to the actual driving speed. Moreover, 
how does driving interfere with the steering, is there any leakage at all? In order to determine this, 
two functions are in place for use within the SRK. 

The function ‘driveTest()’ was written in order to determine how the drive encoder 
readings relate to the angular position of the wheel (if the wheel is viewed as a clock). All this function 
does is to record the difference in shaft encoder readings for a given number of turns completed. This 
observation gives rise to the number of counts per degree for driving the wheel. The function does 
not operate autonomous but rather requires user interaction. The user determines when to start 
and end the observation period. This procedure was conducted several times at different speeds - 


although the speed is not of our concern at this point. The results are given in Table 4.3. 


driving at speed 0x0800 (1 turn) 
Wheel 1 Wheel 2 Wheel 3 Wheel 4 
count per turn -102746 -103949 -105340 -104038 
counts per degree -285.41 -288.75  -292.61 -288.99 


time per turn (sec) 10.85 10.63 10.97 10.87 
drive count for 1 turn n/a 
driving at speed 0x0800 (averaged over 3 turns) 
Wheel 1 Wheel 2 Wheel3 Wheel 4 
count per turn -103989 -104303 -103967 -104229 
counts per degree -288.86  -289.73  -288.80 -298.53 
time per turn (sec) 10.85 10.63 10.97 10.87 
drive count for 1 turn n/a 
driving at speed 0x2000 (averaged over 10 turns) 
Wheel 1 Wheel 2  Wheel3 Wheel 4 
count per turn -103756 -104143 -104812 -104705 
counts per degree -288.21 -289.29 -291.15 -290.85 
time per turn (sec) 2.704 2.698. 2.729 PAINE 
drive count for 1 turn n/a 
driving at speed 0x2000 (averaged over 100 turns) 
Wheel 1 Wheel 2 Wheel3 Wheel 4 
count per turn -104377 -102594 -104440 -104435 
counts per degree -289.92  -284.98  -290.11 -290.10 











time per turn (sec) Dat 2 271 2.02 22 
drive count for 1 turn | 63394.94 63297.88 63331.94 63337.61 





Table 4.3: Data obtained for determining drive parameters with program ‘driveTest()’. 


It can be seen from the Table that the number of counts per degree for all wheels is given 


by approximately 290 counts/degree except for wheel two at the commanded speed of 0x0800. 
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However, it is assumed that the user simply failed in observing the correct number of turns for this 
wheel. Another test run eventually with even more turns should be conducted. However, for ease of 
computation and in agreement to Mays/Reid [1], it is expected that for a given number of encoder 
counts, all wheels will advance by exact the same angle if commanded by the same digit and the 


conversion is given by 





Table 4.4: Conversion Factor for Driving all Wheels. 


In asecond step, a function ‘velocityTest()’ was implemented in the source file ‘motor.c’ 
in order to determine the driving speed as a function of servo data sent to the driving servos. The 


inner workings of this function are quite simple: 


1. Align all wheels, set speed = 500. 
. Set all motors to speed. 


. Wait one second to let servos attain steady state. 


m WwW bh 


. Observe the difference in shaft encoder readings for an observation period of one second. 
Store the readings in main memory (starting at 0x00100000) at consecutive locations. 


On 


. Decrease speed = speed -10. 
6. If speed < -500 stop, otherwise repeat the loop with step 2. 


7. Stop the test program. 


Once the program was done, the data (steering and driving delta for every second) was 
downloaded as an ASCII dump to the notebook, converted to decimals and further analyzed using 
the MATLAB function ‘velocity.m’. Although it was - based on the results from Mays/Reid 
- expected to obtain a nonlinear relationship between the velocity (which is proportinal to the 
difference in encoder readings) and the commanded digits, the results proved to be quite different. 

For free floating wheels, the drive encoder advances for a given speed during the time interval 
of 1 sec are shown in Figure 4.1 and the equivalent steer encoder differences are shown in Figure 4.2. 
To solidify the results, a second experiment, now with the vehicle on the ground has been conducted. 
The results according to this experiment are shown in Figure 4.3 and Figure 4.4. 

As can be seen from the graphs, both experiments show the same linear relationship for the 
driving of all wheels with just slightly changing parameters and in addition to this, the interaction 
from driving to steering for each wheel is insignificant and can be neglected. The test was conducted 


a total of three times, two times with the wheels on the ground and the vehicle moving in a straight 
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Wheel 1, fit digits = 1.3311832e-002 driveDelta + —1.72896 
500 


digits to servo 
digits to servo 
o 





-3 -2 -1 0 1 2 3 —3 -2 -1 0 1 2 3 
velocity (drrveDelta) {counts/sec] x 10° velocity (driveDelta) {counts/sec] x 10° 


Wheel 4, fit digits = -1.3315856e-002 driveDeita + 0.60989 


digits to servo 
So 
digits to servo 





-3 -2 -1 0 1 2 3 -3 -2 -1 0 1 2 a) 
velocity (driveDelta) {counts/sec} x 10° velocity (driveDelta) {counts/sec} x 10° 


Figure 4.1: Commanded Digits versus Encoder Differences for Free Floating Wheels. 


Wheel 1 


steerDelta [counts/sec] 
steerDelta {counts/sec] 





-500 0 500 -500 0 500 


digits to servo digits to servo 
Wheel 4 Wheel 3 





steerDelta [counts/sec] 
steerDelta [counts/sec] 





-500 0 500 500 
digits to servo digits to servo 


Figure 4.2: Influence of Commanded Drive Digits on Steering Wheels. Plot shows Encoder Differ- 
ences vs. Commanded Drive Digits for Steering Motors (Steering Motors set to zero). 
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Wheel 2, tit digits = - 1.330107 1e-002 driveDelta + -1.75192 


digits to servo 
° 
digits to servo 





-500 
-3 -2 -1 0 1 2 3 ~3 -2 -1 0 1 2 3 
velocity (driveDelta) [counts/sec] x 10° velocity (drive Delta) [counts/sec} x 10° 
Wheel 4, fit digits = -1.3314266e-002 driveDelta + 0.47206 Wheel 3, fit digits = 1.3292298e-002 driveDelta + -0.14845 


digits to servo 
digits to servo 
° 





: he : 
-3 -2 -1 0 1 2 3 -3 -2 -1 0 1 2 3 
velocity (driveDelta) [counts/sec] x 10° velocity (driveDelta) [counts/sec] x 10° 


Figure 4.3: Commanded Digits versus Encoder Differences for Vehicle on the Ground. 
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Figure 4.4: Influence of Commanded Drive Digits on Steering Wheels for Vehicle on the Ground. 


Plot shows Encoder Differences vs. Commanded Drive Digits for Steering Motors (Steering Motors 
set to zero). 
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line and a third time with the vehicle lifted off the ground and the wheels rotating free. Despite 
the changing test conditions, the results were independent from the way the vehicle was suspended. 
The recorded data for each wheel was fitted in a least square sense by a polynomial of order 1 (a 
straight line) and the coefficients are given in Table 4.5 where the encoder difference driveDelta is 
given in units of counts per second. 

Wheel 1 
Wheel 2 


Wheel 3 
Wheel 4 



















0.01331 driveDelta [count/sec] 4 
digit = -0.01330 driveDelta [count/sec] - 1.65 
digit = 0.01329 driveDelta [count/sec] - 0.30 
-~0.01331 driveDelta [count/sec] + 0.55 





Table 4.5: Relationship between drive encoder difference and commanded servo drive speeds. 


It is beneficial to use the relationship digit=f(driveDelta/sec) vice the inverse since for any 
motion control process, we are given the desired speed (which is directly proportional to the variable 
driveDelta/sec) and want to obtain the required digit to control the servos accordingly. Using the 
conversion factor given for driving the wheels (see Table 4.4) and the wheel’s radius (which we 
assume to be equal for all wheels to be 18.9cm) we obtain the conversion from distance travelled to 


count advances by 


27 _3 
1 count = 360 « 290 -18.9 cm = 1.13747-10 cm 


87914 counts (4.1) 


lm 
and we finally end up with a handy relationship between velocity [cm/sec] and digits commanded 
to the servos (the digits are not yet left justified): 


= 11.70 v [cm/sec] 
11.69 v [em/sec] 


11.68 v [cm/sec] 
11.70 v [cm/sec] 





Table 4.6: Relationship between Velocity [cm/sec] and Commanded Servo Digit (needs further be 
multiplied by 16 to justify left). 


After multiplying the above data by 16 in order to shift it digital wise one nibble to the left, 
we obtain 

Table 4.7 yields the values that can be directly sent to the driving servos. They will already 
yield the left-justified data sent to the analog output board. Recall that only the upper 12 bit 
determine the final servo speed. Hence, when driving the wheels, we encounter a discretization error 


introduced by converting the double valued velocity to 12 bit! 
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B. LINEAR MOTION PROFILE 


In order to test the sampling results obtained from both, the shaft encoder and the IMU, 
a simple linear motion profile was implemented in the SRK. The profile is implemented as routine 
‘linearMotioni()’ in the source file ‘motor.c’ and is shown in Figure 4.5. As it turned out later, 
this profile was not suitable to obtain reliable data. Hence, a second profile was implemented as 
routine ‘linearMotion2()’ and the vehicle’s principle behavior is depicted in Figure 4.6. While 
the vehicle would travel a distance of 4 m in forward direction and return to its start position 
upon execution of ‘linearMotion1()’, it would travel for 5/6 of a meter forward and stop for 
‘linearMotion2()’. However, the vehicles maximum acceleration for the former motion would be 
2 cm/sec? while for the latter, the vehicle would speed up to 1 m/sec* which is quite high! 

In the following, the results for the shaft encoders for both motion profiles will be discussed 
utilizing the motion control procedure as outlined in Chapter II on page 12. The analyzing MATLAB 


routine ‘shaft.m’ is for completeness given in Appendix B.5 on page 65. 


is Linear Motion Profile #1 


This motion segment lasts for a total of 70 seconds, after which the vehicle is expected to 
have returned to its start position. The stop during the period 30sec < t < 40sec is utilized to mark 
the turning position for the vehicle. 

Clearly, as Figure 4.8 reveals, the driving angles are off by up to 10 degrees upon completion 
of the motion program. On the floor, a lateral deviation of approximately 35 cm has been observed. 
The longitudinal distances traveled came out to be 395 cm for the forward leg and 401 cm for the 
reverse leg. 

Despite the fact that the steering motors are set to zero, there remains interaction between 
driving and steering. It needs to be determined whether or not this relates to badly adjusted (offset) 
servo motors or indeed driving interaction. In any case, it is quite evident that feedback is required 


to provide the desired accuracy for straight motion. The aspects of feedback are not discussed in 


Wheel 1 = 187.20 v [cm/sec]} 
Wheel 2 187.04 v [cm/sec] 


Wheel 3 186.88 v [cm/sec] 
Wheel 4 187.20 v [cm/sec] 





Table 4.7: Relationship between Velocity [cm/sec] and Commanded Servo Digit. 
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Figure 4.5: Linear motion profile implemented as linearMotion1(). 
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Figure 4.6: Linear motion profile implemented as linearMotion2(). 
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Figure 4.7: Accumulated drive encoder readings versus time for linear motion profile #1. 
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Figure 4.8: Accumulated steer encoder readings versus time for linear motion profile #1. 
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this thesis. However, Mays/Reid [1] provide a brief discussion about this topic. 


2: Linear Motion Profile #2 


In order to serve the IMU analysis better, a linear motion profile was needed which provided 
a greater acceleration for the vegicle. Thus, the linear motion program ‘linearMotion2()’ has been 
implemented in the file ‘motor.c’. This motion program drives the vehicle over a distance of about 
83 cm (5/6 m) within 4 sec. As was for the motion profile #1, the vehicle follows closely the 
determined path. 

Considering the fact that no feedback has been implemented in the motion control programs, 
it can be concluded that the shaft encoder readings provide sufficient accuracy for determining the 


planar motion for SHEPHERD under the condition that no slip occurs. 


C. UNCERTAINTIES IN MOTION CONTROL 


It is quite obvious that the accuracy of the motion control part and the position determina- 
tion depends on several parameters that may vary over time or that were determined too inaccurate. 
The main reasons for inaccurate motion control and position determination derived from the shaft 


encoder readings are 


1. Inaccurate sensor parameters relating to the angular position of each motor. 


2. Wheel radius not measured correctly or radius changing over time due to wear or changing 
tire pressure. 


3. Data reduction for velocity from double valued data type to 12 bit that are being sent to 
the servos. 


All these factors will eventually degrade the performance of the implemented routines. Hence, there 


will be ample space for improvement for future work. 
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Figure 4.9: Compounded drive encoder readings versus time for linear motion profile #2. 
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Figure 4.10: Compounded steer encoder readings versus time for linear motion profile #2. 


V. INERTIAL MEASUREMENT UNIT 


This chapter describes the framework that was implemented on SHEPHERD in an attempt 
to obtain reliable velocity and position data based on inertial measurements. All source code as it 
pertains to the implementation of the Inertial Measurement Unit (IMU) is provided in the source 
file ‘imu.c’ and listed in Appendix C.1 starting at page 67. 

Figure 5.1 shows the vehicle’s basic appearance with the four wheels at the corners labelled 
1 to 4 and the motion sensor with its three corners marked by a solid dot which span the xy-plane 
in the body frame {B} mounted on its steel plate. The solid dots on the sensor’s casing are just to 


relate the upside down orientation to the general appearance as given by Figure 5.2. 





Figure 5.1: Configuration for Shepherd Rotary Vehicle 


Due to the particular design of the SHEPHERD Rotary Vehicle, the vertical axes of each 
wheel are exactly located on the corners of a square of dimension 0.8 x 0.8 m. The sensor is mounted 


upside down below the supporting steel plate at the location indicated in Figure 5.1. 
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A. INERTIAL SENSOR 


For this project, a four degree of freedom inertial sensor cluster (Solid-State Motion Sensor, 
Type MotionPak) from SYSTRON Donner, Concord California [8] is being used. It provides three 
outputs for linear motion measured with servo accelerators (a;,@,,@,) and one output for measuring 
rotational motion about the z-axis (w,). This data comprises a cartesian coordinate system which 
is shown in Figure 5.2. The dots in the three corners shall help identify the attitude of the sensor 


as shown in Figure 5.1. 


i 
Qz 


Figure 5.2: Axis orientation for MotionPak Sensor 


The MotionPak is customized by the manufacturer for the anticipated dynamic range. Ta- 


ble 5.1 shows most of the specifications as they apply to the model in use. 


Sse Sar on) 

(Range [eg [tg [ag | 28sec 
19.881mV](deg] sec) 

(ae 


$3.78 V 
Bandwidth 369 Hz 938 Hz 869 Hz 
Noise (10-100Hz) 


Table 5.1: Operating specifications for MotionPak Model No. MP-G-CQBBB-100, Serial No. 0329 
(after Reference [9]) 





As was already shown by Figure 2.4 on page 8, the analog data provided by the MotionPak 
IMU is converted into digital data by an A/D-Board interfacing to the VMEBus. The converted 
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digital data is transferred from the A/D-Board to the 68040 processor on the TUARUS board via 
the VMEBus. Figure 5.3 shows how the four analog channels from the MotionPak IMU are actually 
routed through the A/D-Board to the CPU. 











Data block from dual port RAM 


ay 16 bit (12 bit data, left justified) 


ay 
Software trigger (every 10ms)} 


Az 


We 


VIC 068 


Figure 5.3: IMU Hardware Integration 


B. A/D CONVERSION SCHEME 


The IMU provides continuous analog data to channels 1 to 4 of the A/D-Board VME9325 
[10]. With every 10 ms timer interrupt, a block conversion on the AD-Board is triggered via software 
command issued by the interrupt handling routine from the 10 ms timer. The AD-Board is configured 
to multiplex the four input channels every 50 yu sec for a total of 200 samples. Thus, in a consecutive 
order, each of the four channels are sampled at a sampling rate of f,=5000 Hz and the digital data 
is stored sequentially in the A/D-Boards dual-port RAM. Once the block conversion is complete, 
the A/D-Board will issue an interrupt (see Appendix D.4 on page 93 for the exact interrupt level 
in use) to 68040 where the corresponding interrupt handler routine analyzeVME9325 () preprocesses 


(filters) the block data and stores it as the most recent data in the global variables 


a, => imuAX 
ay => imuAY 


a, => imuAZ 
Ww, => imuOmegaZ 
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which will thus be available for the next motion control cycle to update the actual vehicle motion. 


The board’s status can be observed by means of LED indicator lights at the boards front panel: 


Green LED | Red LED 


off on Board is not initialized 
on on 
off off 
on off 


Board undergoes initialization 
Table 5.2: Status indicator lights for A/D-Board 









Board is initialized but inactive 
Board is performing A/D block conversions 





At present, the data is merely downloaded via the TAURUSBug ’du0’ option (see Ap- 
pendix D.3) through the CONSOLE port to the Laptop and from there to the UNIX System, where 
the data was further analyzed using MATLAB. However, for the future, the sampled data would be 
directly processed by the 68040 processor as outlined above. 

One might ask, why was the odd sampling frequency f, = 5000 Hz is being used instead of 
a more intuitive 10 kHz. A look at the timing diagram Figure 5.4, reveals that the time A between 
the last block conversion (w, in block 50) and the start of the next motion control cycle is governed 
by the sampling frequency: for continuous sampling (e.g., increased block number to transfer), the 
larger f,; the smaller will A be. However, there is a constraint on the minimum length of A due 
to the fact that the sampling block data must be transferred to the TAURUS main memory. This 
transfer must be done before the next motion control cylce is issued by the 10 ms timer interrupt. 


This rule must be closely followed, otherwise a loss of sampling data might occur. 


Timer Interrupt k Timer Interrupt k+1 
a eee 
0 10 
’ t [me] 
5| | 
| Block 1 Block 2 Block 50 | Block 1 
Au ay az We az ay az Wy az ay az We lan ay az We 
LS A t [ms] 
50us8 : 
2003 
+ 


Figure 5.4: Timing Diagram for A/D-Board 


The A/D-Board maps a preset input span of A = 20 V for a differential input range of + 
10 V into n=12 bit bipolar two’s complement data left justified in a 16 bit word. The value of -2048 


relates to an analog input equvalent of -10 V < Zanatog < -9.99512 V. Likewise, the digital output 
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of 2048 relates to 0 V < Zanaltog < 0.00488 V. The stepsize is given byxde= = = a = 4.88 mV. To 
make use of the maximum range available, the board provides a variable gain to amplify the input 
signal by factors G=1, G=2, G=4, or G=8. Moreover, we need to scale the data by the appropriate 
scaling factors 5 for each channel which are given in Table 5.1. Thus, for a given channel with gain 
G and scaling 5, we obtain the analog equivalent of the data by shifting the digital value Ldigitat DY 


4 bit to the right (which is equivalent to a division by 16) and than re-scale it according to: 


A 
Lanalog = mGSs (ldigerar = 2048) 


Using the scaling factors given in Table 5.1 we end up with the units of [g] for az,a,, and a, and 
[degrees/sec] for w,. Expressing the linear acceleration a in terms of the gravitational acceleration 
g rather than in SI-units of [m/sec*] turns out to be beneficial if we need to find the Euler angles 


and a suitable representation for it in the reference frame {R}. 


C. SCHEME FOR DATA ANALYSIS 


Accelerometers sense the sum of the gravitational acceleration d, and the linear acceleration 


a which is due to an external force applied to the body in the body frame {B} 


Ae =. a ee (5.1) 
which relates to the reference frame {R} as 
o-a— “a £ wu. (5.2) 


In both frames, g is the acceleration of gravity derived from Keplerian physics for two body motion 
theory between the Earth and a body. Usually, g is a function of the distance r between the center 


of masses of the two bodies and can be computed with 


GM 
g= 





= 
with the constants G and M as described in Appendix A. For a body at the Earth’s surface, 
g = 9.81 m/sec’ and usually, the variation in height for small changes can be neglected. Therefore 
we will not concern ourselves with a variable g and assume that g = 9.81 m/sec’. 

In the following, we will devise a scheme to eliminate the undesired gravity components in 
our measurement data. Therefore, we will have to focus on the stationary vehicle first, that is, the 
only acceleration acting on the vehicle in frame {B} will be the Earth gravity. Moreover, we know 


that in the reference frame {R}, the acceleration due to gravity has only a +z-component whereas 
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in {B} we would usually encounter gravitational components in each of the principal axes unless the 


sensor is perfectly aligned with frame {R}: 


0 aes 
"g=] 0 and “B= | "gy 
g Pie 


subject to the constraint that g = ,/®g2 + °g2 + ®g?. To express frame {B} in terms of frame {R} 


we make use of the rotation matrix as outlined in the previous sections and given by Equation 3.2: 
Paes = R eee 


We therefore do need to get the Euler Angles (roll, pitch, and yaw) as defined on page 17. We make 
us of the fact that the acceleration of a stationary sensor as measured in {R} should only display 


the gravitation: 


0 
“im =| 0 | ="Ra(i) *Ry(#) "Rx(0)"Em 
) 


Solving for °a,, yields 
"aim = “Rx () "Ry (4) *Rz (8) "Sm 
We recall the identity given in Equation 3.1 on page 16 and rewrite the above equation in terms of 


the transpose of each rotation matrix: 
Pain = "Ra() "Ry (¢) *Rz (6) am (5.3) 


For any measurement vector °a,, and the related vector "a4 in frame {R}, Equation 5.3 together 
with the definitions for the rotation matrices Equation 3.3, Equation 3.4 and Equation 3.5 given on 
page 17 provides us with a system of three equations from which we can determine the Euler Angles. 


In particular, we are easily able to determine the Euler angles as a function of the measurement 


BS: 
Qam = —9G sin(?¢) (5.4) 
Qym = g sin(@) cos(¢) (5.5) 
Qzm =  gcos(@) cos(¢) ; (5.6) 


We recognize that for the stationary data, the acceleration measured in {B} does not depend on the 


yaw angle w which is directly related to the heading of the vehicle (in order to obtain the heading, we, 
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of course, would need to have a compass at hand). Solving the above system for the two remaining 


Euler angles yields the following equations: 


@ = -—arcsin (“= (5.7) 
a 
or alternatively for @ 
6 = = arcsin Py 


———— (5.9) 
WICH ae CE 


We see that the last two equations both yield a solution for 6. Depending on the accuracy of our 
measurements and the accuracy of the desired math functions we have implemented so far, we may 
prefer the one to the other. Since the Sensor’s output data is already scaled with respect to g, the 
Earth’s gravity (see Table 5.1), we may prefer the former and discard Equation 5.9. This is reflected 
in the MATLAB listing for ‘getdata.m’ where the data is arranged accordingly. 

Based on the theory pertaining to the inertial measurement sensor as outlined above, the 


following scheme to obtain the position data for the vehicle is proposed: 
1. Sample stationary data (as is usually the case if one starts up the vehicle) in frame {B} for 
a certain period of time. 
2. Filter the data with an appropriate lowpass filter. 
3. Compute the Euler angles @ and ¢. 


4. Transform the data from frame {B} to frame {R} using the rotation matrices given by 
Equation 3.2, use arbitrary yaw angle w. 


5. Subtract the acceleration due to gravity acting on the vehicle to obtain the sole acceleration 
due to a specific force given in frame {R}. 


6. Integrate the data in a suitable way to find the velocity and position vector of the vehicle. 


D. INTEGRATION TOOLS 


In our analysis of the inertial measurement sensor, we will have to integrate the data in order 
to arrive at the velocity vector. There are many integration methods available for integrating discrete 
data. For equispaced, discrete data, most of the more commonly known integration formulas such 
as the Trapezoidal rule, Simpson’s Rule, ... are based on the Newton-Cotes Integration Formulas 


({11],[12]). Given a set of values f(z;) for equispaced z; =a+ihVi=0...n with h = %, the 
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integral of f(x) on the interval [a,b] can be approximated by 


[ ed: = [ Pater 


where P,,(x) is the Lagrangian polynomial that passes through all the points x; and the interval 
(a, b] is covered by the (n+1) equidistant points z;. P,(x) is given by 
Tl 
2G) = SU lean 
c= 


where qa; is given by 





qT 
aE 

ai = |] 
Li— LE 





ie 
If we let z =a+hs the above integral for P,(x) reduces to a simple sum 
b = b-ac 
n(z) dr =h i) a= le: ; 
| Pale) ae Yi fle) a= 2 Vases (5.10) 


The values for ns and o; can be computed given the above relations. However, we will not concern 


ourselves with this issue and state the results for the first few parameters: 


Tips [a SS™S~S~*dCCommomd'y Known ule 
al 2 ial 


Tapezoidal 
141 Simpson’s 1/3 
ilies) al Simpson’s 3/8 


feo 2ud2 32 7 
19 75 50 S0m/s 19 
840 | 41 216 27 272 27 216 41 





Table 5.3: Newton-Cotes Formula Parameters 


Some of these formulas are being implemented in the function ‘integral.m’ on page page 65 
and used for integrating the acceleration data. The analysis in the following sections will discuss 


which formula shall be preferred to the others. 


E. DATA FILTERING AND COMPUTATION OF POSITION VECTOR 


Several recordings for stationary data have been taken. In the process of obtaining the 
position vector for the vehicle we would expect that starting, say from an initial position (0, 0,0), rR}, 
this should not vary much as time passes by. 

Initially, the sampling scheme was such that each channel of the JMU was sampled at a 
sampling rate of 100 Hz with every 10 ms timer interval. Later on, this has been changed to a 


sampling rate of 5000 Hz as shown in the timing diagram Figure 5.4 on page 34. 
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Le Stationary Data Analysis 


The data collected for the stationary data analysis in this subsection has been sampled 
prior to changing the sampling frequency from 100 Hz to 5000 Hz. Thus, this is reflected in the 
data presented in this subsection. In addition, the IMU at this stage was not yet mounted to the 
vehicle and the orientation of the axes was such that the sensors z-axis pointed up instead of down 
as shown in Figure 5.1. Figures 5.5 to 5.10 show typical results obtained. They show data recorded 
and processed for a stationary vehicle with file ‘imu.m’ (see Appendix B.1 on page 59). The data 
was recorded on the fifth floor of Spanagel Hall with the sensor titled by a significant amount which 
was not further specified. 

As can be seen from Figure 5.6, the linear components (az, ay, and a,) contain distinct 
sinusoidal components at f = 20Hz and f = 40Hz. The origin of this behavior still needs further 
examination. However, it seems not to be related to the block sampling interval of T=10 ms, rather 
than to vibrations inherent in the building. These sinusoidal components can not be beneficial to 
the performance of our compuations. Therefore, we have to eliminate the residues by some suitable 
filtering technique. 

In the time domain (Figure 5.5), we see the effect due to the A/D sampling process: the 
sampled data obtained through the A/D Board truly displays the characteristics for discrete-time 
signals. Moreover, since the sensor was titled, the data will reflect the values according to this 
orientation relative to frame {R}. Thus, the next step involves computation of the Euler angles and 
transforming the data into frame {R} using the results obtained in Equation 3.2. Now, follwoing the 
transformation the data for az and a, should ideally go to zero (at least in the mean). The result 
is shown in Figure 5.7 with its Fourier spectrum given by Figure 5.8. 

In fact, the acceleration for a, and ay is almost zero whereas the acceleration for a, is almost 
—1.0 g (the DC component is not shown in the frequency spectrum. The negative sign for this data 
set is due to the fact that the sensor’s z-axis pointed down. The final step is to obtain the velocity 
and the position by integrating the acceleration once or twice, respectively. The velocity is shown in 
Figure 5.9. As can be seen from the plot, the velocity in x- and z-direction pretty much approaches 
steady-state after about 3 sec of recording whereas the velocity in y-direction approaches steady state 
after about 10 seconds (eventually, a longer recording needs to be taken to verify this statement). As 
for the position vector, which is shown in Figure 5.10, we see that during the first second the error is 
small and the position remains pretty much zero. However, as the velocity assumes its steady state, 
the position displays a linear behavior. Therefore, based on the stationary analysis, it is advisable 


to update (reset) the navigation solution based on the IMU at least every second. Even better, if 
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Figure 5.5: Time domain behavior for linear acceleration and angular velocity for the stationary and 
tilted IMU as measured by the A/D-Board (normalized to units [g]) in frame {S}. 
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Figure 5.6: Fourier spectrum for linear acceleration and angular velocity for the stationary and tilted 
IMU as measured by the A/D-Board (normalized to units [g]) in frame {S}. 
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Figure 5.7: Time domain behavior for linear acceleration and angular velocity for the stationary and 
tilted IMU as measured by the A/D-Board (normalized to units [g]) in the reference frame {R}. 
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Figure 5.8: Fourier spectrum for linear acceleration and angular velocity for the stationary and tilted 
IMU as measured by the A/D-Board (normalized to units [g]) in the reference frame {R}. 
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the Euler angles which represent the attitude of the vehicle could be determined continuously and 
in accordance to the updated Euler angles, new rotation matrices would have to be determined on 


a regular basis. 


2. Non-stationary Data Analysis with Profile #1 


In the sequel, we will analyze data sampled at a sampling frequency of f, = 5000 kHz 
according to the timing diagram depicted in Figure 5.4 from an IMU that is mounted on SHEPHERD 
as shown in Figure 5.1. First, in order to correlate the sampled data to the actual motion of 
the sensor/vehicle, the same linear test motion profile as introduced in Chapter IV and shown in 
Figure 4.5 on page 27 was being utilized. Due to the vast amount of data that had to be analyzed 
(a recording for 70 sec at a sampling frequency of 5000 Hz on four IMU channels comprised a mere 
2.8 MByte!) the analysis was performed on segments of data in order not to exploit the limits of 
computational power. In particular, to enhance the performance of the built in MATLAB Fourier 
transform function, segments contained 65536 samples, which is a power of two (2!°). 

Figure 5.11 depicts the linear acceleration as determined by the IMU. Despite the fact that 
the linear m:  « profile was only along the x-axis of the vehicle, the sensor seemed not to distinguish 
between the channels. All three components display some sort of noise and the signals do not at all 
seem to be related to the actual motion profile. 

The detailed analysis of the a,-channel is given in Figure 5.12 and 5.13 for the time frame 
0<t< 13sec. Figure 5.12 shows that the original data is distorted throughout the entire frequency 
range. Moreover, the time signal does not display the expected behavior according to the true motion 
profile. Instead, the oscillations increase in amplitude as time advances. To reduce the noise, an 
elliptic filter has been used to attenuate the noise in the stopband. The software filter, implemented 


using MATLAB’s built in signal processing functions, had the following specifications: 


1. Passband from 0...20 Hz with max. attenuation of 0.1 dB 


2. Stopband from 50... Hz with min. attenuation of 80 dB 


Other filters such as Chebychev and Butterworth filters were also being tested. None of 
these filter types showed a significant improvement of the data. The only advantage Butterworth or 
Chebychev filters have compared to Elliptic filters is a better phase linearity in the passband. On the 
other hand, and most important for an implementation where computation time is scarce, Elliptic 


filters are most efficient since they yield the smallest-order filter for a given set of specifications [14]. 
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Figure 5.9: Velocity data integrated from the linear acceleration in frame {R}. 
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Figure 5.10: Position integrated from the velocity in frame {R}. 
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Figure 5.11: Linear Acceleration measured by all three channels of the IMU for Linear Motion Profile 
#1. 
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Figure 5.12: Analysis of linear acceleration ax as measured by the IMU. 
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Figure 5.13: Analysis of linear acceleration ax after data has been filtered by a 6th order elliptic 
filter with passband edge at 20 Hz and Stopband edge at 50 Hz. 
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The results, as depicted in Figure 5.13 do not look too promising. Althought the filter 
achieved to smooth the data and reduce the noise, it could not ensure that the acceleration would 
show any transition at t=10sec. Recall that according to the true profile, the acceleration should 
be zero starting with t=10sec. The only reason that can be attributed to this fatal behavior is 
the dynamic input range of the A/D-Board: operating the accelerometer at a maximum linear 
acceleration of a; = 0.02m/sec? (which is only + 0.002 g) we utilize only a voltage span from -7.6 
mV to +7.6 mV that is fed into the A/D-Board. Even if the maximum gain of 8 is used to amplify 
this signal, the amplitude would never exceed = 62 mV which comprises a mere four digits in the 


digital output range. 


3° Non-stationary Data Analysis with Profile #2 


It was anticipated that, for the second motion profile as shown in Figure 4.6, results for the 
measured acceleration would improve. The maximum acceleration was set to be 1.0 m/sec? with 
the maximum velocity reached by the vehicle to be = 0.5 m/s. The sampled data for all three linear 
acceleration channels is shown in Figure 5.14. The plot reveals strong interaction between all three 
channels. One goal would be to get rid of these interferences by means of a suitable filter technique. 
For the time being, we focus on the a,-channel. The time and frequency behavior for the x-channel 
is depicted in Figure 5.15. Strong harmonic components influence the overall performance and a 
similarity to the actual motion can not be found. 

Upon filtering with an elliptic filter of order 6, the recorded data can somewhat be related 
to the true motion. However, since the sharp edges in the ideal acceleration profile (Figure 4.6) 
result in high frequency components of the signal, these edges can not be recognized by the IMU 
(the cutoff frequency for the linear accelerometers is around 900 Hz, see Table 5.1. Nonetheless, the 
questions remains: would this be suffice to compute the velocity? We refer to Figure 5.16 and see 
that the velocity does in principle follow the curve depicted by the ideal motion profile Figure 4.6. 
As soon as the recognizable motion kicks in, the velocity seems to be distorted by an offset in the 


acceleration data (rather than assuming a=0 on the interval t € [2, 3] sec). 


A. Non-stationary Data Analysis with Profile #3 


To get rid of the lowpass constraint, a third motion profile has been developed. The profile 


is shown in Figure 5.17. 


46 


Linear acceleration 


a, {g] 





2 
Angular velocity 


omega, [rad/sec] 





0 0.5 1 15 2 2s 3 35 
t[s} 


Figure 5.14: Linear Acceleration and angular velocity w, relative to frame {R} measured by the 
IMU for Linear Motion Profile #2. 
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Figure 5.15: Analysis of linear acceleration ax as measured by the IMU. 
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Figure 5.16: Analysis of linear acceleration ax after data has been filtered by a 6th order elliptic 
filter with passband edge at 20 Hz and Stopband edge at 50 Hz. 
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Figure 5.17: Linear Motion Profile #3. 
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Clearly, this motion should only contain low frequency components. As was the case for the 
other two motion profiles, the IMU senses noise in all three channels even though the motion takes 


place only in the sensors x-direction. 


F. SUMMARY 


Based on the results obtained from the linear motion profiles #1 .. #3 the following con- 
clusions for the implementation of the inertial measurement unit can be drawn: First, the IMU data 
sampled off the IMU needs to fit appropriately in the A/D-Boards input range. As a crude rule of 
thumb based on the observations made in this Chapter, the time average of the acceleration signals 
to be A/D-converted (this may include any additional gain) should be at least 1/10 th of the max. 
allowable input amplitude of the A/D-Board (e.g., at present, the max. input is + 10 V, the input 
signal should be at least 1 V in magnitude). A more detailed analysis is required in this respect. 
Probably the most effective solution would be to utilize MotionPak Accelerometers (QFA7000) with 
current output rather than voltage output. In this case, the output could be scaled by the user 
to especially lower ‘g’ limits by means of variable scaling resistors (see [13] for more information). 
Probably the most significant shortfall in the design of the vehicle was determined to be the variable 
suspension of the vehicle’s wheels. Whenever the vehicle accelerates by a significant amount, the 
vehicle’s steel platform may tilt. This change of attitude will be recognized by the IMU but can not 
be attributed to a change of the vehicle’s main body attitude and thus to a change of position in 


3D space. 
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Figure 5.18: Analysis of linear acceleration ax as measured by the IMU. 


0.1 butane . 6 . i eS SB 


0.05 


ax (9) 
Oo 


~0.05 oe e : LAB Mesa 


0 1 2 3 4 5 6 7 
t [sec] 


0.15 


0.1 


~0.05 





t [sec] 


Figure 5.19: Analysis after Elliptic Filtering (6th order filter) with passband edge at 20 Hz and 
Stopband edge at 50 Hz. 
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VI. SENSOR FUSION 


Having developed the two independent navigation components in the previous Chapters, it 
was anticipated to fuse the data provided by both systems to further improve the accuracy of the 
navigation system. However, since the performance of the IMU does not yield any reliable motion 
data, sensors fusion at this point of time is obsolete. Some literature research has been done to 
obtain a hint as to how to fuse the data. Almost all papers related to sensor fusion utilize the 
extended Kalman filter. Welch [16] provides a decent introduction in Kalman filtering. Nonetheless, 
it is anticipated that Neural Networks might be applicable to this problem as well. Thus, the aspect 


of sensor fusion will be left for future work. 
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VIL. CONCLUSIONS AND RECOMMENDATIONS 
FOR FUTURE WORK 


A. CONCLUSIONS 


The research issues addressed by this thesis were 


Implement the hardware and software for an Inertial Measurement Unit 


Implement the software for a shaft encoder system 
e Evaluate the performance for both sensors 


Sensor Fusion 


Both the IMU and the shaft encoder systems have been implemented in software and hardware. The 
sampling frequency for the A/D-Board was set to be 5 kHz. Both systems have been tested with 
three different linear motion profiles. 

The work conducted in addressing the first of these topics revealed several sources of nav- 
igation inaccuracy. The A/D Converter board currently in use does not match the IMU’s output 
range for accelerations below about 1 m/sec”. In addition, due to the vehicle’s sophisticated wheel 
suspension, the IMU’s attitude control could not be related to the attitude of the vehicle and was 
changing with time as the vehicle moved. This introduced a slowly varying and yet significant error 
in numerically integrating the acceleration. 

The second issue addressed proved to be less difficult. Decent results have been obtained 
for th elinear motion under the condition that no slip occurs and the vehicle’s position can be 
determined to within 0.5 percent accuracy. 

The overall motion control system seems to be stable at all. However, it has been observed 
that computation power for the 68040 processor is scarce. This is mainly to the fact that a public 
domain GCC Compiler is in use for generating the executable code. This compiler does not seem 
to generate optimal executable code. In addition, the lack of a math processor and math library 
functions required that semi-optimal trigonometric functions be implemented in the source code as 


well, introducing further inaccuracies. 


B. RECOMMENDATIONS FOR FUTURE WORK 


There are many issues that. were briefly addressed in this thesis but could not be investigated 


in detail. Much work needs to be done in the following areas. 


oO 


. Determine the optimal resolution for the A/D-Board based on the anticipated motion pro- 
files. 


. Investigate whether or not variable gain control for the IMU data would improve the per- 
formance of the IMU. 


. Develop a scheme for attitude control vice changing the vehicle’s suspension. 


. Implement the filter algorithms as determined in this thesis. Care needs to be taken that 
computation time is crucial and efficient computation methods be used. 


. Implement an Input/Output Kernel utilizing the 68030 processor for online debugging, 
display of status information, and eventually off-loading of some of the lower priority task 
such as transferring data between boards. 


. Investigate how the system presented in this thesis would work for most general type of 
motion including rotational motion and motion in three dimensions. 
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APPENDIX A: CONSTANTS 


Table 1.1: Constants used throughout the text 
Universal constant of gravitation G=6.672-10~1! go» 
Mass of Earth M=5.98 - 1074 kg 
mean Earth radius R,=6.371 - 10° m 


of 
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APPENDIX B: MATLAB M-FILES 


This appendix contains essential MATLAB M-Files that are being referenced in the text. 


i IMU.M 


The MATLAAB file ‘imu.m’ is used to analyze the data recorded from the IMU. It makes 
use of the MATLAB functions ‘filter1’, ‘euleri1.m’ and ‘integral’ which are listed following 


this section. 


1 function imu(fname,G,T,f) 

2 
nn ne 
4 % function imu(fname,G,T,f) 

BSS 
6 % 

7 % M-File to obtain reliable position data. Procedure: 

Bf 

9 % 1. load data and scale data 

10 % 2. plot data in frame {B} 

11 % 3. filter data with butterworth LP filter in frame {B} 

12 % 4. determine Euler angles and transform data fto frame {R} 

13 % 5. integrate data to obtain velocity 

14 % 

15 % Author: Thorsten Leonardy 

16 ¥% Date: 10/23/97 

17 % Compiler: MATLAB V4.21c 

18 7% 

19 % Input: fname = name of data file 
20 7% G = gain sequence for channels, default [1 1 1 4] 
7 iy A note that G(3) includes the orientation of the 
Zz. 6, IMU’s z-axis (>0 is up, <O is down) 
23 4h, 1 = sampling time for data 
Za Oh f = switch for filtering ax data 

ee ee 
26 
27) «g=9.81; % local gravitational constant [g=9.81m/s~2] 
28 
29 if nargin<2 
30 G=[1 1 1 4]; % sample gain 

31 T=0.01; % samples per block and channel 

32 f=0; % do not filter data 
33 end 
34 


35 up = G(3)/abs(G(3)) % determine if IMU’s z-axis points up 
36 G(3)=abs (G(3)); 


38 % load data, ax,ay and az are in [m/sec”2] or [g], wz is in [rad/sec] 
39 [t,ax,ay,az,wz]=getdata(fname,G,T); 


40 

41 disp(’>>> Plot data in {B} ...’) 

42 plotdata(t,ax,ay,az,wz) ; % plot data 

43 

44 disp(’>>> Transform {B} --> {R} ...’) 

45 [ax,ay,az]=euleri(ax,ay,az,up) ; %, transform data to reference frame {A} 
46 

47 disp(’>>> Plot data in {R} ...’) 

48 plotdata(t,ax,ay,az,wz); % plot data in {R} 

49 

50 disp(’>>> Integrate data in {R} to obtain v ...’) 

51 ([tv,vx]=integral(t,g*ax,1); % integrate step by step 
52 ([tv,vy]=integral(t,g*ay,1); % integrate step by step 
53 [tv,vz]=integral(t,g*(az-up) ,1); % integrate step by step 


59 


54 
55 
56 
57 
58 
59 
60 


62 
63 
64 
65 
66 
67 
68 
69 
70 
ig 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
ot 
92 
93 
94 
95 
96 
97 
98 
99 
100 
101 
102 
103 
104 
105 
106 
107 
108 
109 
110 
Lit 
112 
113 
114 
115 
116 
1i7 
118 
119 
120 
121 
122 
123 
124 
125 
126 


figure 

myplot(tv,vx,’Velocity in frame {R}’,’’,’v_x [m/sec]’,(3 1 1]) 
myplotity,vy,’’,’*?,’v.y [m/sec]’,(3 1 2}) 

myplot(tv,vz,’’,’t [sec]’,’v_z [m/sec]’,(3 1 3]) 


disp(’>>> Integrate data in {R} to obtain position ...’) 


(tp,xJ=integral(tv,vx,1); % integrate step by step 
(tp, yJ=integral(tv,vy,1); 4 integrate step by step 
(tp,z]=integral(tv,vz,1); 4 integrate step by step 
figure 


myplot(tp,x, Position in frame {R}’,’’,’x ([m]’,(3 1 1]) 
muplotctp,y,°’s'’,’y im)?, 3 1 2)) 
myplotito,z,°”,’?t [eecl’,*z (m)’,[3 1 3]) 


af of 
mx=mean (ax) ; % compute the mean 
my=mean (ay) ; % compute the mean 
mz=mean (az) ; %4 compute the mean 


% compute the FFT 
(AX, fJ=filter1 (ax,6,t(2)-t(1)); 


mAX=AX (1); % obtain the mean 
AX(1)=0; % suppress dc component 
figure 


myplot(t,ax,({’Acceleration ax in frame {R}, mean is ’ num2str(mx)],’t [sec]’,’ax [g]’,(3 1 1]) 


myplot(£,AX,(?FFT for ax [g], mean is AX(f=0)= ° num2str(mAX) ’? [g], fs=5000 Hz’],... 
“4 (Bz)? ,° Ax Les? (3 tf 2)) 


% zoom on in for f=0..50 Hz 

ix=find(£<=50) ; 

myplot(f(ix) ,AX(ix),’Blow up view for FFT for ax [g]’,... 
>¢ (Hz)’,’AX (g)’,(3 1 3)) 


af=filteri (ax,10,20/2500,50/2500,0.1,80); % Cauer filter 


figure 
myplot(t,af,’Acceleration ax in frame {R} after elliptic filtering’,... 
*t taec)?,*ax ([¢)*,02 1°1)) 


(t,vJ=integral (t,af,6); 


myplot(t,v,’Velocity vx in frame {R} after elliptic filtering’,... 
?t [sec]’?,’vx [m/s]’,[2 1 2]) 


end % of if f 


disp(’>>> Plot all figures to disk in postscript format as ’’fname_xxx.ps’’’) 
for 1=1:gcf 

figure (i) 

eval((’print -dps2 ’ fname ’_’ num2str(i) ’.ps’]) 
end 
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a4 FILTER1.M 


The file ‘filteri’ provides a set of suitable filter routines such as an FFT, Chebychev or 


Butterworth filter, and more. 


1 function [y,f]=filterl(x,type,a,b,c,d) 
aden I 
3 % function [y,f)=filter1(x,type,a,b,c,d) 
EE I RS a a a a a a 
5 % Author: Thorsten Leonardy 
6 % Date: 10/16/97 
7 % Compiler: MATLAB V4.2c1 
8 % 
9 % Input: x = input data matrix (M+#N) 
10 7% type = utility function (filter) to apply 
ie a..d = parameter used for some filter types 
12 % 
13 % type 2..4 average across the rows: 
14 % type = 2 simple mean 
1 type = 3 average using Simpson’s 3/8 rule 
16 % type = 4 average using Simpson’s 1/3 rule on 9 samples 
a7 (fh type = 5 average using trapezoidal rule 
18 % type 6 operate on each row: 
19 ¥ type = 6 obtain Fourier transform (a is the sample interval in [sec]) 
20 % type 7 ... 9 operate on first row only: 
zi Ud type = 7 moving average FIR-Filter (n Taps] 
a2 4, type = 8 Butterworth filter (wp, ws,Rp,Rs] 
23 % type = 9 Chebychev Filter (wp,ws,Rp,Rs] 
24 % type = 10 Elliptic (Caver Filter) ([wp,ws,Rp,Rs] 
25 4 
26 % Output y = output data (M*N2/2), 
27 4 N2 is a power of two closest to and less or equal to N 
28 % f = frequency scale (1*N2/2) for y if type=10 
en ee == 
30 
31 disp([’*** Function "filteri", type ’ num2str(type) ’ *+**’]) 
32 
33 if type== 
34 y=x; 
35 return 
36 end 
37 


38 % compute mean of the sampled data from the channel 
39 if type== 
40 y=x(a,:); 


41 end 

42 

43 if type==2 
44 y=mean (x) ; 
45 end 

46 

47 if type== 


48 c=(3/8)*(1 332332331); 
49 y=c*x/9; 


50 end 
51 
52 if type== 


53 c=(1/3)*(1 424242 41); 
54 y=c*x(1:9,:)/8; 


55 end 
56 
57 if type== 


58 ez(1/2)*(1 222222 2 21); 
59 y=c*x/9; 


60 end 

61 

62 nn rr tt tt re ren er ere ren nm 
63 % Fourier Transform of x 

C9 mm ne ee we 
65 

66 if type==6 

67 

68 T=a; %4 sampling time of data 


61 


69 
70 
71 
72 
73 
74 
75 
76 
a, 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
pe 
92 
93 
94 
95 
96 
o7 
98 
99 
100 
101 
102 
103 
104 
105 
106 
107 
108 
109 
110 
ns Nl 
112 
113 
114 
115 
116 
Diy: 
118 
119 
120 
121 
122 
123 
124 
125 
126 
Pate 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 
138 
139 
140 
141 
142 
143 
144 


F=1/T; % sampling frequency [Hz] of signal 
=mean(x’); % mean of data sequence 
N=size(x,2); % total length of data 
N2=2° (floor(log(N)/log(2))) % reduced length to power of two 
x(:,N2+1:N)=[]J; % cut off the data sequence 
t=T*(0:N2-1); % time base corresponding to data 
f=linspace(0,F,N2); 4 frequency base 
% Matlab computes the Fourier transform of a signal that is sampled 
4 at a sampling frequency fs. The corresponding frequency scale is 
% expressed in terms of the digital frequency omega=2*pi*(f/fs) in 
% the range 0..2*pi (any discrete FT is periodic in terms of omega 
% with period 2*pi). 
y=abs(fft(x’))’; % compute the Fourier Transfor of x(t) 
f(:,N2/2+1:N2)=(); % discard redundant frequency part 
y(:,N2/2+1:N2)=(); % discard redundant upper half of spectrum 
% X(w) relates now to w=(0,pi] 
y=y/N2; % normalize the amplitude 
end 
Lo eae EEE moving average FIR filter *¥4K Ea EEEEKEEE 


if type== 


if nargin<3 
P=5; 

end 

M=P; 

N=size(x,2); 


r=x (15. % filter only first row 


x=x~{ zeros(1,1+M) x(1:N-1-M)]; % the delay 
x=x/ (14M) ; 


y=zeros(1,N); 
y(i)=x(1); 


for i=2:N 
y (i)=y(i-1)+x(i); 


end 


end 


x=x(1,:); % filter only first row 


% filter specifications (digital frequencies) 
4 e.g. if fs=2000Hz and passband edge is supposed to be at fp=500 Hz, 
% parameter wp must be wp=fp/ (fs/2)=500/(2000/2)=0.5 !!! 


wp=a; %4 wp is passband edge [0..1] where 1 relates to fp/(fs/2) ... 
ws=b; % stopband edge ... 

Rp=c; % ... and max. attenuation [dB] at passband edge 

Rs=d; % ... and min. attenuation [dB] at stopband edge 


{N,wc]=buttord(wp,ws,Rp,Rs); % filter order and 3dB cutoff-frequency 
disp((’Butterworth filter order ’ num2str(N)]) 


“filter process 
(b,aJ=butter(N,we); % compute the filter coefficients 
y=filter(b,a,x); % filter the data 


end 


(ceo ee a 
% Chebychev Type II filter 
Ph pe a ce eee eee ee ee 
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145 xex(157)'; % filter only first row 


146 

147 % filter specifications (digital frequencies) 

148 74 e.g. if fs=2000Hz and passband edge is supposed to be at fp=500 Hz, 
149 % parameter wp must be wp=fp/(fs/2)=500/(2000/2)=0.5 !!! 

150 wp=a; “4 wp is passband edge [0..1] where 1 relates to fp/(fs/2) 
151 WS=D; % stopband edge ... 

152 Rp=c; 4... and max. attenuation [dB] at passband edge 

153 Rs=d; 4%... and min. attenuation [dB] at stopband edge 

154 

155 (N,wn]=cheb2ord(wp,ws,Rp,Rs); % filter order and 3dB cutoff-frequency 
156 disp([’Chebychev Type II filter order ’ num2str(N)]) 

157 

158 [b,a]=cheby2(N,Rs,wn) ; % compute the filter coefficients 
159 y=filter(b,a,x); % filter the data 

160 

161 end 

162 

163 ¥, ---------------------------------------------------- 

164 % Elliptic filter (Cauer filter) 
nn nn ea 

166 if type==10 

167 

168 x=x(1,:); 4 filter only first row 

169 

170 “4 filter specifications (digital frequencies) 

slik 4 e.g. if fs=2000Hz and passband edge is supposed to be at fp=500 Hz, 
172 4% parameter wp must be wp=fp/(fs/2)=500/ (2000/2)=0.5 !!! 

173 wp=a; % wp is passband edge [0..1] where 1 relates to fp/(fs/2) 
174 ws=b; %4 stopband edge ... 

175 Rp=c; % ... and max. attenuation [dB] at passband edge 

176 Rs=d; 4 ..- and min. attenuation [dB] at stopband edge 

Did 

178 (N,Wn]=ellipord(wp,ws,Rp,Rs); % filter order and 3dB cutoff-frequency 
179 disp([’Elliptic filter order ’ num2str(N)]) 

180 

181 (b,aJ=ellip(N,Rp,Rs,Wn) ; 7, compute the filter coefficients 
182 y=filter(b,a,x); % filter the data 

183 

184 end 

185 

186 

187 return 

188 

DB ly rn nn rn nn see rene nares sesess= 
190 % end of ’filteri.m’ 

ee ee ee ee eee ee ee 


3. EULER1.M 


The function ‘euler1.m’ is used to convert the recorded IMU data which is given in the 


sensor frame {S} to the reference frame {R} by means of rotation matrices. 
1 function [ax,ay,az]=euleri(ax,ay,az,up) 


Pe 

I a a age a a a 
4 % function [ax,ay,az]=euleri1(ax,ay,az,up) 

BN a oe me wn em nen eres eS er = ana n nan aa aaeeeenn— a Saas 
6 4 

7 % M-File for computing the Euler angles for a given set of data 

8 % measured in the sensor frame {S} and transforming the data into 

9 ¥% the reference frame {R}. 

10 % 

11 % Author: Thorsten Leonardy 

12 % Date: 10/16/97 

13. % Compiler: MATLAB V4.21c 

14 4, 

15 % Input: ax(1,N) = acceleration [g] in {S} ax-direction 

16 7% ay(1,N) = acceleration [g] in {S} ay-direction 
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17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 


33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 


52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 


N) = acceleration [g] in {S} az-direction 
= orientation of sensors z-axis (+1=up,-1=down) 


eration relative to frame {R} 


Measurement matrix aS(3,N) relative to Frame {S} 


er angles based on the average 


% acceleration during 2nd second 


% may change this 
% take the mean of first ix values 
% the gravity based on the mean 


disp({’--> mean of g in frame {S} is ’ num2str(g,6) ’ g’]) 


vy az(1, 
vA up 

4 

% Return: accel 
OF See eae 
% put data into one 
aS=[ax;ay;az]; 

4, ee en Oa aa eae 
% determine the Eul 
2 i a ee ae a as as a cs ene 
ix=101: 200; 
m=mean(aS(:,ix)’); 
g=sqrt (m+*m’); 
psi=0.0; 


phi=-asin(m(1)); 
theta=asin(m(2)/cos 


phi=up*phi ; 
theta=up*theta; 


4 psi, arbitrary value 
% phi 
(phi)); % theta 


disp([{’--> Theta (roll) is ’ num2str(theta*180/pi,7) ’ degrees’]) 


disp({’--> Phi (pit 
disp([’--> Psi (yaw 


ch) is ’ num2str(phi*180/pi,7) ? degrees’]) 
) is ’ num2str(psi*180/pi,7) ’ degrees’]}) 


% compute elements of the rotation matrix 
% complete rotation matrix would be R=RZ*RY*RX 


RX=[ 1 0 0 5 % rotation matrix about X_A 
0 cos(theta) -sin(theta) ; 
0 sin(theta) cos(theta) ]; 
RY=[ cos(phi) 0 sin (phi) 3 % rotation matrix about Y_A 
0 1 0 ; 
-sin (phi) 0 cos(phi) J; 
RZ=[ cos(psi) -sin(psi) 0 : % rotation matrix about Z_A 
sin(psi) cos(psi) 0 ; 
0) ) 1 J; 


aR=RX*aS; ‘ 
aR=RY*aR; V8 
aR=RZ*aR; % 


m=mean(aR(: ,ix)’); 
g=sqrt (m*m’) ; 
disp(({’--> mean of 


ax=aR(1,:); 
ay=aR(2, Fy) ’ 
az=aR(3,:); 


rotate {B} about {R} x-aris 
rotate new {B} about {R} y-axis 
rotate new {B} about {R} z-axis 


% take the mean of first ix values 


% the gravity based on the mean 
g in frame {A} is ’ num2str(g,6) ’ g’]) 
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4. 


INTEGRAL.M 


This function implements the Newton-Cotes integration formulas as described in the text. 


This provides an easy means to compare the results for different integration schemes. 
function [(t,y]=integral(t,x,n) 


D. 


22a ee Oe OO oO ae Ss OE SB SB Se OOS SSF SSO SC ee Se See SS See CS Soe eo FSB ese 8 SF Be eee ee Oe eS eee ee 


function (t,y]=integral(t,x,n) 


Integrates the input x based on the Newton-Cotes algorithn. 
The integral is computed on each column. 


n = the number of panels (n panels require n+1 data points) 
t is the time base corresponding to the data. 


SS Sees Be Se Be eM eS eM em ewe ee RK Se ee eB eS Be Se HS EE REE SS SR SSS SS HS RSH RR eS SS SS 


(N,c]=size(t) 


if 


(c>N) 
x=x’?; t=t’; N=c; % need data as a vector, N=length of data 


end 


vi 

if 
if 
if 
if 
if 
it 


prepare the coefficients in the sum formula 
(n==1),c=({1 1]/2; end 
(n==2),c=(1 2 1]/6; end 
(n==3) ,c=(1 3 3 1]/8; end 
(n==4) ,c=(7 32 12 32 7]/90; end 
(n==5) ,c=(19 75 50 50 75 19]/288; end 
(n==6),c=(41 216 27 272 27 216 41]/840; end 


c=n*(t(2)-t(1))*c; 


fo 


r i=i:n:N-n 
x(i,:}=#c*x(i:itn,:); % store result in place 


end 


hf 
t= 


cumsum(x(i:n:N-n,:)); 
t(n+1i:n:N); % return the time scale 


te et eed ed ed ee 


% End of ’integral.m’ 


% 


SHAFT.M 


In order to analyze the shaft encoder data that was recorded during the different motion 


programs. 


function shaft (fname) 


OANA ASF WN Ee 


10 


12 
13 
14 
15 
16 


18 


M-File to analyze the shaft encoder readings recorded for SHEPHERD’s 
motion according to the different motion profiles. 


Author: Thorsten Leonardy 

Date: 11/11/97 

Compiler: MATLAB V4.21ic 

Input: fname = name of data file (no extension ’*.dat’) 


e.g. at the prompt >>shaft(?linear4’) 


load data 
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ts) 
20 
21 
22 
23 
24 
25 
26 
27 


29 
30 


32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
fal 
72 
13 
74 
75 


eval(({’load -ascii ’ fname ’.dat, data=’ fname ’;’ fname ’=[];’]) 


% reshape the data 


N=length (data) /8 % number of 10ms intervals contained in data 
data=reshape(data,8,N) ; 
t=0.01*(1:N); % the time base 


driveDelta=data(1:2:8,:)’;  % driving data [counts/10ms] 
steerDelta=data(2:2:8,:)’; % steering data [counts/10ms] 


% account for the fact that drive encoders for wheels 2 and 4 read negative 
%4 differences if wheels are driving forward 
driveDelta(: ,2:2:4)=-driveDelta(: ,2:2:4); 


% accumulate the data to obtain true rotation of motors 
drive=cumsum(driveDelta) ; % the distance travelled 
steer=cumsum(steerDelta) ; %4 the angle steered 


% scale to SI units 
drive=drive/87914; % drive distance in [nm] 
steer=steer/256; % angle steered in degrees 


4 plot data 
figure 
for i=1:4 
if (mod(i,2)) 
subplot (2,2,i+1) 
else 
subplot (2,2,i-1) 
end 
plot(t,drive(:,i)),grid 
title(({’Wheel ’? num2str(i)],’FontSize’,8) 
xlabel(’Time [sec]’,’FontSize’ ,8) 
ylabel(’Drive distance [m]’,’FontSize’,8) 
set (gca, ’FontSize’,6,’Box’,’off’) 
a=axis; a(3)=min(drive(:,i)); a(€4)=max(drive(:,i)); axis(a) 
end 
eval(({’print ~dps2 shaft’ num2str(gcf) ’.ps’]) 


figure 
plot(t,steer),grid 
title(’Steer values for Wheels 1..4 with steer value set to zero’,’FontSize’ ,8) 
xlabel(’Time [sec]’, ’FontSize’,8) 
ylabel(’Steer angle [degrees]’, ’FontSize’ ,8) 
set(gca, ’FontSize’ ,6, Box’, ’off’) 
ix=min(find(t>=65)); 
for i1=1:4 
text(t(ix),steer(ix,i),[{’Wheel’ num2str(i)J,... 
*HorizontalAlign’,’left’,’VerticalAlign’, ’top’,’FontSize’ ,6) 
end 
eval((’print -dps2 shaft’ num2str(gcf) ’.ps’]) 
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APPENDIX C: GCC COMPILER SOURCE-FILES 


This appendix lists the C-source code that is being referred to throughout the text. Each 


individual source file was written in C and crosscompiled using the GCC Compiler Version 2.72 with 


the following command line: 


x 


gcc -c -m68040 -o filename.o filename.c 





IMU.C 


The file ‘imu.c’ provides all the routines required to implement the inertial measurement 


sensor as outlined in Chapter V. Moreover, they provide the interface for further development of 


the system. 

QOL mr ne nn renner erernresrs sis * 
2 * * 
a «6 File: Tone. C * 
4 * ok 
5 * Environment: GCC Compiler v2.7.2 * 
6 * Last update: i0 September 1997 * 
7 * Name: Thorsten Leonardy * 
8 »* Purpose: Provides routines required for controlling the inertial * 
9 * measurement sensor. * 
10. —s* * 
ii * Compiled: >gcec -c -m68040 -o navigat.o navigat.c * 
12 * * 
ee ee ee eae */ 
14 

I a lah ar R BAG De 6 on ee 
16 

17 Here is how the routines work: 

18 

19 1. Make sure that initVME9325 is called inside main() 

20 this will setup the proper interrupt handling for reading data 

21 from the accelerometer. 

Ze 

23 2. A/D-Block conversions as specified in initVME9325 will be initiated with every 

24 10ms timer interrupt. However, to make the data available, make sure that 

25 interrupt for conversion complete are being issued: 

26 

Aa 3. Call startVME9325 to enable block conversion complete interrupts 

28 on IRQ-5 to 68040 processor and therefore copy data into main memory 

29 

30 4. To seize copying data into main memory, call stopVME9325 

31 

32 5. The A/D converter is setup such that after every i10ms timer interrupt 

33 a block conversion will be initiated. A total of AD_NUM_CONVERSIONS 

34 conversions will be performed on the four channels on the IMU 

35 in the sequence CHO, CHi, CH2, CH3, CHO, ... 

36 The sample time is set to be 25us (hence, one specific channel will 

37 be sampled every i100us) 

38 

39 6. If interrupts are enabled, the most recent data obtained with every 

40 10ms timer interrupt will be stored in the structure imu as defined 

41 in SHEPHERD. H# 
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42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
$7 
58 
59 
60 


62 
63 
64 
65 


67 
68 


70 
71 
r(?- 
73 
74 
75 


(ay 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
91 
92 
93 
94 
95 
96 
97 
98 
ce) 
100 
101 
102 
103 
104 
105 
106 
107 
108 
109 
110 
111 
i2 
113 
114 
115 
116 
117 


ie 


The boards status can be observed at the front panel: 

(a) green LED is on -> board performs A/D-Conversions, interrupts enabled 
(b) green LED is off -> board performs A/D-Conversions, interrupts disabled 
(c) red LED is toggling -> Interrupts are being handled by the handler, 


data is read from board into SHEPHERD main memory 


(d) red LED is on/off -> interrupt handler is not being called 


rn rn en en nn rn nn er enn nnnnnn-- «/ 

#include "shepherd.h" 

#include "imu.h" 

int adCounter; /* counter for debugging purposes */ 

int mainMemCounter; /* to count the data stored in main memory */ 

/* the next is used as temporary storage for analyzing acceleration DATA */ 

unsigned short *mainMemData; 

[mr rr rn rn ere nner nnne-- * 
* initVME9325 (void) * 
* *x 
* Environment: GCC Compiler v2.7.2 * 
* Last update: 24 July 1997 * 
* Name: Thorsten Leonardy * 
* * 
* Purpose: Initializes AD-Board VME9325. Board will convert * 
* analog data from channels specified and store the respec- +* 
* tive digital data (2 Bytes per channel, 12 bit data, lowest * 
x nibble is zero) sequentially in dual port ram. * 
*x x: 
* Board will operate in Block mode with interrupts and timed * 
* periodic triggering (10ms cycle). E.g. perform 10 conver- * 
& sions on each of the four channels. Once 40 conversions are * 
* made, initiate interrupt to read data into main memory and * 
* eventually smooth/filter data. * 
BE aE 
OO ew mw ww ww wr nw wr rw ow rr rn rn rw oe rr et nn en ern eo nee ene ee = * / 

void init VME9325 (void) 

ul 

unsigned char *ad = (unsigned char*) VME9325_BASE; /* base address */ 

unsigned char *vmeICR4 = (unsigned char*)VIC_IRQ4; /* VME ICR IRQ-4+/ 

long *vadr; /* for Vector base address */ 

* (ad+0x81)=0x10; /* software reset */ 

* (ad+0x81) =0x02; /* turn both LEDs on to indicate board undergoes */ 
/* initialization */ 

[nn ew no ne ee een ee eea= *x 

* Interrupt settings for VIC * 

KO weer wn ww re rn oe ne ee ee ee ee eee */ 
vadr=(long*)Oxffe40158;  /* VBA address for interrupt handler (4 * 0x56 = 0x158) */ 
*vadr= (long) handlerVME9325; /* write address of handler into Vector Table */ 
/* set up VIC interface for VME-Bus interrupts to TUARUS. AD-Board asserts 

'/* IRQ-4 upon interrupt to VME-Bus. Route as IRQ-2 to MC68040. CAUTION !!! */ 
/* make sure jumper J7 on AD-Board is set correctly !!! 
*xvmeICR4=0x82; /* disable VME-Bus IRQ4 input, route as IRQ-2 to Processor */ 
* (ad+0x83) =0x56; /* interrupt vector number provided by board to VIC */ 
/* program scan sequence (may wish to arrange channels to be scanned differently) */ 
/* channels are scanned, converted and stored in memory in this order + / 
/* * (ad+0x87)=0x00; /* channel O (ax, +-7.5V input range, gain x1) */ 
/* * (ad+0x87)=0x01 ; /* channel 1 (ay, +-7.5V input range, gain x1) */ 
* (ad+0x87) =0x60; /* channel O (ax, +-7.5V input range, gain x8) */ 
* (ad+0x87) =0x61 ; /* channel 1 (ay, +-7.5V input range, gain x8) */ 
* (ad+0x87) =0x02; /* channel 2 (az, +-7.5V input range, gain x1) */ 
* (ad+0x87) =0xe¢3; /* channel 3 (wy, +-2.5V input range, gain x4) */ 
/* gain x4 to cover max. input range +-10V, */ 
/* set EOS bit to indicate end of scan sequence*/ 
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118 
119 
120 
121 
122 
123 
124 
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 
138 
139 
140 
141 
142 
143 
144 
145 
146 
147 
148 
149 
150 
151 
152 
153 
154 
155 
156 
157 
158 
159 
160 
161 
162 
163 
164 
165 
166 
167 
168 
169 
170 
171 
172 
173 
174 
175 
176 
177 
178 
179 
180 
181 
182 
183 
184 
185 
186 
187 
188 
189 
190 
191 
192 
193 


/* setup Board Control Register */ 
* (ad+0x85)=0x08 ; /* enable timer circuit, enable interrupts */ 
/* block mode, software initiates very first trigger */ 


/* setup timed periodic triggering circuit for 5Ousec ( T= 10 * 10 / 2 MHz )x/ 


* (ad+Ox8f )=0x54; /* setup counter to receive single byte prescaler count */ 
* (ad+0x8b)=0x0A; /* load prescaler value into Timer Prescaler Register */ 
* (ad+Ox8f)=0x94; /* setup counter to receive single byte timer count */ 
* (ad+0x8d)=0x0A; /* load Conversion Timer Register * / 


/* load conversion count register *f 
*((unsigned short *) (ad+0x90) )=200; 


/* initialization is complete */ 
* (ad+0x81)=0x01; /* turn off both LED, disable interrupts */ 


sioOut(0,"A/D-Board initialized\n\r"); 


return; 
} /* end of AD_Init */ 


[FR mm rr rrr tn rer rn rr nnn escen= * 
* analyzeVME9325 7 
* * 
* Environment: GCC Compiler v2.7.2 * 
* Last update: 24 July 1997 * 
* Name: Thorsten Leonardy * 
* * 
* Purpose: Saves the data for one complete block conversion cycle from * 
* dual-port RAM of A/D~-Board to Shepherd’s main memory. * 
* In the future, this routine shall be utilized to analyze * 
* and filter the data and save only the filtered data. * 
* This is called from the interrupt handler routine * 
* AD_Handler. * 
* * 
Pn a et et tn ete ete tet ee teeter ron * f 

void analyzeVME9325 (void) 

if 

unsigned short *ad; /* base address for data */ 
int i; 
unsigned short adData[AD_NUM_CONVERSIONS] ; 
ad=(unsigned short*)VME9325_DATA; /* load base address for dual port RAM */ 
FOR eee * 
* here goes the filtering ... * 
Bo a a a a nn rn nn nn nner */ 
if ((adCounter%5)==0) 
toggleVME((unsigned char *)0xfd800000,0x01); /* toggle red LED every 50 msec*/ 
adCounter++; 
YOR rt et rn nen errata * 
* This is temporary backup * 
Wm a ee rr tet nara */ 
for (i=0; i<AD_NUM_CONVERSIONS; i++) { 
adData[i}=*ad++; /* neglect lower nibble */ 
*mainMemData++=adData[i]J; /* save data in main memory */ 
} 
#Hifdef 0 
/* once data is filtered, store obtained values in imu */ 
imu.ax=adData [0]; 
imu.ay=adData[1]J ; 
imu. az=adData[2}, 
imu. omega_z=adData[3] ; 
#tendif 


/* reload start conversion register for next block conversion */ 
ad=(unsigned short*)0xfd800090; /* address for SCR */ 
*ad=AD_NUM_CONVERSIONS ; /* reload register */ 
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return; 
} /* end of analyzeVME9325 */ 


ae ne rn ne een Seeee= * 
* start VME9325 (void) * 
* * 
* Environment: GCC Compiler v2.7.2 * 
* Last update: 10 September 1997 * 
* Name: Thorsten Leonardy * 
* * 
x Purpose: enables interrupts issued by the VME9325 board. * 
* te 
* Called from: whatever function. * 
* * 
Bn re renee eeeeec= aaa eetenteteateteatan! * / 

void startVME9325 (void) 

{ 


unsigned char *statusRegister=(unsigned char *)VME9325_BASE+0x0081 ; 
unsigned char *vmeICR4 = (unsigned char*)VIC_IRQ4; /* VME ICR IRQ-4*/ 


/* initialize global variables ... */ 


mainMemData=(unsigned short *)IMU_DATA_ADR; /* start address for data storage */ 
adCounter=0; /* counter for debugging purposes */ 


*vmeICR4=0x02; /* enable VME-Bus IRQ4 input, route as IRQ-2 to Processor 
/* write status register to enable interrupt and turn off red LED 


*statusRegister=0x09; /* turn off both LEDs, enable interrupts 


return; 
} /* end of startVME9325 */ 


[OR eee * 
* stopVME9325 (void) * 
* * 
* Environment: GCC Compiler v2.7.2 * 
x Last update: 10 September 1997 * 
* Name: Thorsten Leonardy * 
* * 
* Purpose: disables interrupts off the VME9325 AD-Board. Yet, board * 
* will still perform A/D-Conversions but data will not be ® 
* made available to the operating system. * 
x Called from: * 
* * 
Hn nt en eee «f/f 

void stopVME9325 (void) 

{ 


unsigned char *statusRegister=(unsigned char *)VME9325_BASE+0x0081 ; 
unsigned char *vmeICR4 = (unsigned char*)VIC_IRQ4; /* VME ICR IRQ-4*/ 


#ifdef 0 
/* initialize global variables ... */ 


*/ 


*/ 
*/ 


mainMemData=(unsigned short *)IMU_DATA_ADR; /* start address for data storage */ 
adCounter=0; /* counter for debugging purposes */ 


#endif 
*xvymeICR4=0x82; /* disable VME-Bus IRQ4 input, route as IRQ-2 to Processor */ 


/* write status register to disable interrupt and turn off red LED */ 
*statusRegister=0x01 ; /* turn off both LEDs, disable interrupts */ 


return; 
} /* end of stopVME9325 */ 


JR kk 
Assembler routines 
ee aK EEK EK EE EK RAKE EEE EE EAE EEE EEE EE a a / 
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270 
271 
272 
273 
274 
275 
276 
ZiT 
278 
279 
280 
281 
282 
283 
284 
285 
286 
287 
288 
289 
290 
291 
ZZ 
293 
294 
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296 
297 
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299 
300 
301 
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310 
$it 
312 
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317 
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326 
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332 
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335 
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338 
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340 
341 
342 
343 
344 
345 


FOR rr er eee === * 
* handlerVME9325 * 
a * 
* Environment: GCC Compiler v2.7.2 * 
* Last update: 10 September 1997 * 
* Name: Thorsten Leonardy * 
& TE 
* Purpose Handles the VME-Bus interrupt request from the A/D-Board. * 
+ * 
te nn one enn een nn eee «f/f 
asm(" 
.even 
-text 
-globl _handlerVME9325 
_handlerVME9325: 
link a6 ,#-184 /* allocate 184 Bytes on stack to save registers «/ 
fsave a60(-184) 
#ifdef 0 
fmovemx fp0-fp7,sp@- /* move floating point registers 80 bit each */ 
fmovel fpcr,spQ@- /* move floating point Control Regioster *f 
fmovel fpsr,sp@- /* move floating point status register «/ 
fmovel fpiar,sp0- /* move floating point Instruction address register */ 
#endif 
moveml d0-d7/a0-a5,sp@- /* save data and address registers (14*4 Byte) */ 
addq.l #1, _adCounter /* increment counter (testing purpose only */ 
move.l1 #0xfd800081,a0 /* load address status register */ 
and.b #Oxfd, (a0) /* turn off green LED */ 
move.l1 #0xfd800090,a0 /* reload start conversion register */ 
move .wW #200, (a0) 
#ifdef 1 
move.l1 #0xfd820000,a0 /* load address for dual port RAM */ 
lea _mainMemData,al 
move.l (a1) ,a2 
clr dO /* loop counter */ 
Loop: 
cmp.1 #199,d0 
ble.b ~proceed 
nop 
bra.b _done 
nop 
_proceed: 
move.w (a0),d1 /* read next two byte of dual port RAM 
nop /* caution: need this due to pipelining */ 
move.w dil, (a2) 
nop 
addq.l #2,a0 /* increment pointer in dual port RAM */ 
addq.l1 #2,a2 /* increment pointer to next main memory location */ 
addq.l #1,d0 /* increment loop counter */ 
bra.b -Lloop 
_done: 
move.1 a2,(al1) /* write back the next main memory location */ 
/* jsr ~analyzeVME9325 /* copy data from A/D-Boards dual-port RAM to main */ 
/* memory and filter, analyze it «/ 
#tendif 
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346 
347 
348 
349 
350 
351 
352 
353 
354 
355 
356 
357 
358 
359 
360 
361 
362 
363 
364 
365 


Z: 


moveml sp0+,d0-d7/a0-a5 


#ifdef 0 
fmovel sp0+,fpiar 
fmovel sp0@+,fpsr 
fmovel sp0+,fpcr 
fmovemx sp0+,fpO-fp7 


#Hendif 
frestore a60(-184) 
unlk a6 
rte 
“) 


[ERK AER EERE EERE EKER EERE EEK EERE EEE EEE EEE EEE EEE EEE EKER REED 


End of imu.c 


KRESS EERE EEE ERE EEE EERE EERE KEKE EKER EEE REE EEE EEE EEE EEE REE ED / 


MOTOR.C 


The file ‘motor.c’ provides the routines required to control the servo motors. Although the 


listing was already given by Mays/Reid [1], some changes had beend done to improve the overall 


execution time. 
/* bt 


// Edward Mays 
// Shepherd project 
// 20 February 1997 


// update: 27 October 1997 Thorsten Leonardy 


t7 -> provide code to detect slip, 

// -> eliminate calls to readDriveEncoders, readSteerEncoders 

// by including code in readEncoders (improves execution speed) 
// -> compute speed and angular velocity immediately inside 

Vid readEncoders. 


// MotionControl 


#include “shepherd.h" 
#include “motor.h" 
#include “movement.h" 
#include "math.h" 


double theta, omega, speed; 
double a, 

dd{4]; 
int timeForTurn[8]; 
short testSpeed=0x0b00; 
double radPerDigit [ARRAY_SIZE) ; 
int ddc=10000,tc=2000; 


int *leoData=(int *)0x00100000; 


void readEncoders() { 


/* 
/* 
/* 
/* 


/* 
/* 


acceleration in cm/sec*2 */ 

driveDelta required for velocity to steer */ 

storage for time it took to rotate 360 degrees [(10ms] */ 
temp variable for changing speed */ 


desired vale for driveDelta */ 


start data storage */ 


readDriveEncoders (driveReadings) ; 
readSteerEncoders (steerReadings) ; 


} 


void readDriveEncoders (unsigned long int array{]) 


at 


unsigned char *p=(unsigned char*)VMECTR1, cl, ¢2, c3; 


int ix; 
long int temp; 
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for (ix=0; ix<4; ix++) { /* read all four motors subsequentially */ 


* (p+3) =0x03; /* load output latch from counter */ 
* (p+3)=0x01; /* control register, initialize two-bit output latch */ 


/* read three bytes for specific counter ix and save in status */ 
/* first access to Qutput Latch Register reads least significant */ 
/* byte first «/ 


i) 


cl *(p+1) & Ox00ff; 

c2 = *(p+1) & Ox00ff; 

c3 = *(p+1) & OxO00fF; 

array[ix] = ((unsigned int)c1)| ((umsigned int)c2 << 8) | 
(Cunsigned int)c3 << 16); 


p=p+4; /* increment pointer for next counter */ 


} 
return; 
} /* end of readDriveEncoders */ 


void readSteerEncoders(unsigned long int array[]) 


{ 
unsigned char *p=(unsigned char*) (VMECTR1 + 0x0100), c1, c2, ¢3; 
int ix; 


for (ix=0; ix<4; ix++) {  /* read all four motors subsequentially */ 
* (p+3) =0x03; /* load output latch from counter */ 


* (p+3) =0x01; /* control register, initialize two-bit output latch */ 


/* read three bytes for specific counter ix and save in status */ 
/* first access to Output Latch Register reads least significant byte first */ 


ci = *(p+1) & OxO0ff; 
c2 = *(p+1) & Ox00ff; 
c3 = *(p+1) & Ox00ff; 


array[ix] = ((unsigned int)c1)| ((unsigned int)c2 << 8) | 
(Cunsigned int)c3 << 16); 


p=pt4; /* increment pointer for next counter */ 


} 
return; 
} /* end of readSteerEncoders */ 


void computeActualRates() 


{ 


int i; 
double count ,speed; 


for(i=0; 1<=3; i++) 
{ 
if (PreviousCountSpeed[i] == 99999999) /* for derivative for speed */ 
actualSpeeds[i]= 0.0; 
else 
actualSpeeds [i]= 
(convertDifference((driveReadings[i] - PreviousCountSpeed[iJ)) 
*DigitToCmDrive[i}) /DELTA_T; 
PreviousCountSpeed[i] = driveReadings [i]; 


if (PreviousCountSteer[i] == 99999999) /* for derivative for steering */ 
actualAngleRates[iJ= 0.0; 

else 
actualAngleRates[iJ= 


(convertDifference((steerReadings[i] - PreviousCountSteer[i])) 
*digitToRadSteer) /DELTA_T; 
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PreviousCountSteer[i] = steerReadings[i) ; 


} 
I 


void accumulateDriveSpeed() 
{ 


int i; 


for (i=0;1<=3;i++){ 


Display_Speeds[i] += actualSpeeds[i] ; 


return; 


} 


void accumulateDriveSteer() 
{ 


int i; 


for (i=0;i<=3;i++){ 


Display_Steers[i] += 10*actualAngleRates[i]; 
actualAngles[i] += actualAngleRates[i)*DELTA_T; 


return; 


if 


[REE R EEE EEE ERE ERE EERE AERA REAR EERE EEE EERE ERE EE 
Function convertDifference() returns the difference between the new shaft 
encoder position and the old shaft encoder position. The shaft encoder values 
contain only 24 bits (0x000000-Oxffffff). The routine adjusts for the trans- 


ition from Oxffffff to 0x000000 and vice versa. 
KAKA A ERE EEE EE EEA EAE EEE EES EERE EEE EEE EEE EEE RE RR EE / 


int convertDifference(int value) 
{ 
if (value < -0x800000) 
value &= Ox00ffffff; 
else if(value >= 0x800000) 
value |= Oxf£000000; 


return value; 


* 4 %& % 0% & & H HH H HH H 


I; 

[4 enn nn 2-2 3 - $+ = 2 2 2 $5 2 ne nnn = 2 en on + 
* readNewEncoder() 
ak 
* Environment: GCC Compiler v2.7.2 
x Name: Thorsten Leonardy 
* Last update: 10/27/97 
* Purpose: This function reads the counter status for drive and steer 
* motors every 10ms and stores the current values in the 
* variables *’driveReadings’ and ’steerReadings’. In addition, 
* the incremental change to the last update is stored in the 
* variables ’driveDelta’ and ’steerDelta’ to allow for compu- 
* ting the most current speeds and angular velocities. 
x 
* Called from: driver() in movement.c 


+ 


void readNewEncoder() 


{ 


unsigned char *p,*d; 
int: 1x; 


p=(unsigned char*)VMECTR1; /* access steering counter registers 


for (ix=0; ix<4; ix++) { 


/* read all four driving motors sequentially 


driveCountPrevious[ix]=driveCount[ix]; /* save previous value 
steerCountPrevious[ix])=steerCount[ix]; /* save previous value 


/* -------------+--+------------------ +/ 
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/* read drive encoders for wheel ix */ 


[® a2 2-20-22 5 2 ooo eo oe */ 

*(p+3)=0x03; {* 
* (p+3)=0x01; /* 
d=((unsigned char*)&driveCount[ix])+2; /* 
*d-- = *(pt+i) & Ox00ff; /* 
xd-- = *(p+i) & Ox00ff; /* 
*d = *(pti) & Ox00ff; /* 
[Ror nn xf 

/* read steer encoders for wheel ix */ 

[RR creme rn rn re eer reece = */ 

* (p+0x103)=0x03; /* 
*(p+0x103)=0x01; /* 
d=((unsigned char*)&steerCount[ix])+2; /* 
*d-- = *(p+0x101) & Ox00ff; /* 
xd-- = *(p+0x101) & Ox00ff; /* 
*d = *(p+0x101) & Ox00ff; Hs 
p=p+4; /* 


load output latch from counter */ 
initialize two-bit output latch */ 


start with LSB, need offset */ 
read LSB first */ 
read next byte */ 
read most significant byte «/ 


load output latch from counter */ 
initialize two-bit output latch */ 


load LSB first a/ 
read LSB first x / 
read next byte */ 
read most significant byte */ 


increment pointer for next motor*/ 


/* determine difference between previous and current encoder reading */ 
steerDelta[ix]=(steerCount [ix] -steerCountPrevious [ix] )/256; 
driveDelta[ix]=(driveCount [ix] -driveCountPrevious [ix] )/256; 


/* consider the fact that a positive driveDelta for wheels 2 and 4 */ 
/* indicate that wheel is driving backwards !!! Thgus, change sign */ 
driveDelta[ix]=(driveCount [ix]-driveCountPrevious [ix] )/256; 


/* the following is just for testing purposes [leo, 11/17/97] */ 
*encoderDatat++=driveDelta[ix]; /* store in main memory */ 
*encoderDatat++=steerDelta[ix]; /* store in main memory */ 


} /* end of for */ 


/* account for the fact that a positive driveDelta for wheels 2 and 4 */ 
/* indicate that wheel is driving backwards !!! Thus, change sign to */ 
/* obtain a positive driveDelta for wheel driving forward !!! x / 


driveDelta[{iJ=-driveDelta [i]; 
driveDelta[{3]=-driveDelta{3]; 


return; 


} /* end of readNewEncoder */ 


FOR rr rrr errr *x 
* readNewEncoder() * 
* ® 
* Environment: GCC Compiler v2.7.2 * 
x Name: Thorsten Leonardy * 
* Last update: 10/27/97 * 
x Purpose: This function reads the counter status for drive and steer * 
* motors every 10ms and stores the current values in the * 
* variables ’driveReadings’ and ’steerReadings’. In addition, * 
* the incremental change to the last update is stored in the * 
% variables ’driveDelta’ and ’steerDelta’ to allow for compu- * 
* ting the most current speeds and angular velocities. * 
* * 
* Called from: driver() in movement.c * 
Bo ee rn eee «f/f 
void readEncoder() 
{ 
unsigned char *p,*d; 
Int 1x; 
p=(unsigned char*)VMECTRi; /* access steering counter registers x/ 
for (ix=0; ix<4; ix++) { /* read all four driving motors sequentially */ 
driveCountPrevious [{ix]=driveCount [ix]; /* save previous value */ 
steerCountPrevious[ix]=steerCount [ix]; /* save previous value x / 
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} 


% 


vo 


{ 


/* 
/* 
/* 
/* 


[8 ooo nn ee ren nee «/ 
/* read drive encoders for wheel ix */ 
[Reem tr rr er ene eeecccc-- */ 
* (p+3)=0x03; /* load output latch from counter */ 
* (p+3)=0x01; /* initialize two-bit output latch */ 
d=((unsigned char*)&driveCount[ix])+2; /* start with LSB, need offset */ 
*d-- = *(pt1) & Ox00ff; /* read LSB first */ 
*d-- = *(p+1) & Ox00ff; /* read next byte */ 
*d = *(p+i) & Ox00ff, /* read most significant byte */ 
[Reo ee */ 
/* read steer encoders for wheel ix */ 
[RR eer nn rn nnn ne ee *«/ 
* (p+0x103)=0x03; /* load output latch from counter */ 
* (p+0x103)=0x01 ; /* initialize two-bit output latch */ 
d=((unsigned char*)&steerCount [ix])+2; /* load LSB first */ 
*d-- = *(p+0x101) & Ox00ff; /* read LSB first */ 
*d-- = *(p+0x101) & Ox00ff; /* read next byte */ 
*d = *(p+0x101) & Ox00ff; /* read most significant byte */ 
p=p+4; /* increment pointer for next motor*/ 


/* determine difference between previous and current encoder reading */ 


steerDelta[ix]=(steerCount [ix] -steerCountPrevious [ix] )/256; 
driveDelta[ix}=(driveCount [ix] -driveCountPrevious [ix] }/256; 


} /* end of for */ 


/* account for the fact that a positive driveDelta for wheels 2 and 4 */ 
/* indicate that wheel is driving backwards !!! Thus, change sign to +*/ 
/* obtain a positive driveDelta for wheel driving forward !!! «/ 


driveDelta[iJ=-driveDelta[1]; 
driveDelta[3]=-driveDelta[3] ; 


return; 


/* end of readEncoder */ 


computeSpeedAndAngle() 

Environment: GCC Compiler v2.7.2 

Name: Thorsten Leonardy 

Last update: 11/21/97 

Purpose: This function computes the speeds, angles and angular velo- 
city for all four wheels based on the most recent shaft 
encoder readings from readNewEncoder(). 

Called from: driver() in movement.c 


id computeSpeedAndAngle (void) 


Int. 25 


/* compute measured driving speed [cm/sec] and steering angle [rad] and */ 


/* steering rate [rad/sec]. 
for(i=0; i<=3; i++) { 


=7 


actualSpeeds [i] = ((double)driveDelta[i] }*CM_PER_DIGIT/0.01; 
actualAngles [i] += ((double)steerDelta[i])*RAD_PER_DIGIT; 
actualAngleRates[i] = ((double)steerDelta[i])*RAD_PER_DIGIT/O.01; 
} 
return; 
*/ 
Verifies validity of incoming speeds/angles and converts */ 
digitial input for the DA board / 
*/ 
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346 void driveMotors(){ 


347 

348 int ix,Speed_Digit ,Steer_Digit, counter; 

349 double speedi, steerl, temp; 

350 

351 unsigned short bitMask=0x8000; /* access bit 15 for align wheel 1 */ 
352 unsigned short *servoStatus=(unsigned short *) (VME9421+0x00ca); /* digital input */ 
353 

354 bitMask = bitMask >> 3; 

355 

356 /* updateWheelDrive(); wheel values for driving x / 

357 /* updateWheelSteer(); */ 

358 /* comupte the current actual wheel direction in WheelDirAct[] */ 

359 

360 if (mode != 100){ 

361 for(ix =0; ix <ARRAY_SIZE; ix++){ 

362 /* seeeeeeeeaeenaaeeeeasteering/driving interaction*++#sxk eek ex */ 
363 /* here +/- 1/50 of the steering value is added to the driving */ 
364 /* for each specified wheel. Note the negative sign on elements [1] +*/ 
365 /* and (3]provide the same direction driving as elements [0] and [2] */ 
366 

367 Omega_Speed = desiredSpeeds[ix] + 

368 SteerDriveInteract*desiredAngleRates[ix]*18.9; /* cm/sec */ 

369 

370 /* conversion to digits */ 

371 Speed_Digit = velocityReferenceTable(Omega_Speed,ix) + 

o12 DriveFeedBackGain*(Omega_Speed - actualSpeeds[ix]); 

373 Steer_Digit = rateReferenceTable(desiredAngleRates [ix] ) 


374 + steerFeedbackGain*(desiredAngleRates[ix]-actualAngleRates[ix]) 
375 + angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]) ; 


376 

377 if (Speed_Digit>DigitsHigh) /* Limitation +/ 
378 Speed_Digit= DigitsHigh; 
379 if (Steer_Digit>DigitsHigh) 
380 Steer_Digit= DigitsHigh; 
381 if (Speed_Digit<DigitsLow) 
382 Speed_Digit= DigitsLow; 
383 if (Steer_Digit<DigitsLow) 
384 Steer_Digit= DigitsLow; 
385 

386 switch (mode) { 

387 case 2: 

388 case 3: 

389 case 4: 

390 case 5: 

391 case 6: 

392 case 7: 

393 case 8: 

394 case 9: 

395 case 10: 

396 case 11: /* case 11: linear test drive, added 11/03/97 Leo */ 
397 speedDigits[ixJ= (short)Speed_Digit; /* casting to short */ 
398 steerDigits[ixJ= (short)Steer_Digit; 
399 break; 

400 

401 case 1; 

402 speed1 = speedDigits[ix]; 

403 steerl = steerDigits[ix]; 

404 if ( speed1l > 0) speedi--; 

405 if ( speedi < 0) speedi++; 

406 if ( steerl > 0) steeri--; 

407 if ( steeri < 0) steeri+t+; 

408 speedDigits[ix] = speed1; 

409 steerDigits[ix] = steer1; 

410 break; 

411 } /* end switch */ 

412 } /* end for */ 

413 } /* end if */ 

414 else { 

415 for (ix=0; ix<3; ix++){ 
416 steerDigits[ix] = 0; 

417 } 

418 for (ix=0; ix<4; ix++){ 

419 speedDigits[ix] = 0; 

420 } 

421 
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switch(modeTstate) { 

case 0: 
steerDigits(3] = 50+*Flag; 
modeTstate = 1; 
break; 


case 1: 
modeTstate = 2; 
break; 


case 2: 
modeTstate = 3; 
break; 


case 3: 
modeTstate = 4; 
break; 


case 4: 
modeTstate = 5; 
break; 


case 5: 
modeTstate = 6; 
break; 


case 6: 
modeTstate = 7; 
break; 


case 7: 
modeTstate = 8; 
break; 


case 8: 
modeTstate = 9; 
break; 


case 9: 
modeTstate = 10; 
break; 


case 10: 
modeTstate = 11; 
break; 


case 11: 
modeTstate = 12; 
break; 


case 12: 
modeTstate = 13; 
break; 


case 13: 
modeTstate = 14; 
break; 


case 14: 
modeTstate = 15; 
break; 


case 15: 
modeTstate = 16; 
break; 


case 16: 
modeTstate = 17; 
break; 


case 17: 
modeTstate = 18; 
break; 


case 18: 
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modeTstate = 19; 
break; 


case 19: 
if (bitMask&*servoStatus)/* read servo status, */ 
/*wait until wheel aligned */ 
Flag = -Flag; 
modeTstate = 20; 


break; 


case 20: 
steerDigits[3] = 0; 
modeTstate = 21; 
break; 


case 21: 
modeTstate = 22; 
break; 


case 22: 
modeTstate = 23; 
break; 


case 23: 
modeTstate = 24; 
break; 


case 24: 
modeTstate = 25; 
break; 


case 25: 
modeTstate = 26; 
break; 


case 26: 
modeTstate = 27; 
break; 


case 27: 
modeTstate = 0; 
break; 


default : break; 


} /* end switch */ 


} /* end else */ 


#ifdef 0 


driveSteer(steerDigits) ; 
driveSpeeds (speedDigits) ; 


tendif 


/* here is a more efficient way of setteing the speeds [Leo, 11/18/97] */ 
/* instead of using the functions driveSteer and driveSpeeds ... 


setServoSpeed() ; 


}/* end driveMotors */ 


double velocityReferenceTable (double desiredVelocity,int i) 


double inVelocity, 
outVelocity; 


inVelocity=new_abs (desiredVelocity) ; 


if (inVelocity>=0.0 &£& inVelocity<=5.0) 
outVelocity = inVelocity*K1[i]; 


if (inVelocity>5.0 && inVelocity< 8.0) 
outVelocity = inVelocity*K2[(i)] ; 


if (inVelocity>=8.0 && inVelocity<20.0) 
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574 outVelocity = inVelocity*K3[iJ; 


575 

576 if (inVelocity>=20.0 && inVelocity<= 70.0) 

577 outVelocity = inVelocity*K4[iJ; 

578 

579 if (inVelocity>70.0 && inVelocity<K5) 

580 outVelocity = inVelocity*K6[i] ; 

581 

582 if (inVelocity> K5) 

583 out Velocity=1023; 

584 

585 if (desiredVelocity< 0.0) 

586 outVelocity = - outVelocity; 

587 

588 return outVelocity; 

589 } /* end velocityLookupTable */ 

590 

591 

592 double rateReferenceTable(double desiredRate) 

593 { 

594 double inRate, 

595 outDigit; 

596 

597 /*outDigit = new_abs(desiredRate); *//* test only */ 

598 

599 inRate=new_abs(desiredRate) ; 

600 

601 if (inRate<= 5.234) 

602 outDigit = inRate*195.4155 ; 

603 else 

604 outDigit=1023; 

605 

606 

607 if (desiredRate< 0.0) 

608 outDigit = - outDigit; 

609 

610 return outDigit; 

611 } 

612 

613 

614 

B15 [8 <n rn rr rr nr nr 8 Se 8 88 en en nm * 
616 * readQneEncoder () * 
617 * * 
618 * Environment: GCC Compiler v2.7.2 * 
619 * Name: Thorsten Leonardy * 
620 * Last update: 10/27/97 * 
621 * Purpose: Reads only the encoder specified by ’wheel’: * 
622 * wheel = 0 ... 3 reads drive encoder for wheel 1..4 * 
623. * wheel = 4 ... 7 reads steer encoder for wheel 1..4 * 
624 * Note: !!! The data (24 bit) is still left adjusted !!! * 
625 8 errr nnn nn nnn rn nn nr nnn nn nn nnn nn nn nnn nn nnn nn nn nana - */ 
626 void readOneEncoder(int ix, int *data) 

627 { 

628 

629 unsigned char *p,*d; 

630 

631 p=(unsigned char*)VMECTR1; /* access steering register * / 
632 p=pt4*ix; 

633 if (ix>3) p=p+0x0090; /* account for the fact VMECTR2=VMCTR1+0x100 */ 
634 

635 * (p+3) =0x03; /* load output latch from counter */ 
636 * (p+3)=0x01, /* initialize two-bit output latch */ 
637 

638 d=(unsigned char *)data; /* start with LSB, need offset */ 
639 da=d+2; 

640 *d-- = *(pt+1) & Ox00ff; /* read LSB first */ 
641 *d-- = *(p+1) & Ox00ff; /* read next byte «/ 
642 *d = *(ptl) & Ox00ff; /* read most significant byte * / 
643 

644 return; 

645 

646 } /* end of readOneEncoder */ 

647 

648 

649 [6 marr rr a re rr rr een ern wren anne ce nn nnaa * 
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* 


linearMotion() 


* 
* 
Environment: GCC Compiler v2.7.2 * 
Name: Thorsten Leonardy * 
Last update: 10/27/97 + 
Purpose: IMplements a linear motion test profile such that the * 

vehicle is following steps in successive 10sec time * 

intervals. * 
Call: User presses ‘1‘ on the keyboard (see user() in file user.c)* 


void linearMotion1 (void) 


{ 


double vix, viy, v2, viyvixRatio,omega2,omega3, beta,ro,ro2,wheelAngleV; 
int ix,Speed_Digit ,Steer_Digit; 
short *servoQut; 


/* read all shaft encoders */ 
readNewEncoder (); 


/* compute the actual rates, velocities and angles */ 

for (ix=0; ix<4; ix++){ 
driveSpeed[ixJ=driveDelta[ix]+*CM_PER_DIGIT/DELTA_T; /* [cm/s] */ 
steerRate (ixJ=steerDelta[ix] /DELTA_T; 
steerAngle(ix]=steerAngle(ix]+steerDelta[ix] *RAD_PER_DIGIT; 

} /* end of for ... */ 


/* initialize temporary variables */ 
speed=motion.Speed; 
theta=motion. Theta; 
omega=motion. Omega; 


[OR rr ere er eeeees= * 


* body motion (former in movement.c) * 


a=290- /* acceleration is 2cm/sec*2 */ 


if (time<1000) { 
speed=a+time/100.0; /* rise linearly from 0 ..20 cm/sec in 10 secs */ 


I; 


if (time==1000) { 
speed=a*10.0; /* vehicle speed constant for next 10 sec */ 


} 


if (time>=2000) 
if (time<3000) 
speed=a* (3000-time)/100.0; /* decelerate to zero speed for 20sec..30sec */ 


if (time==3000){ : 
speed=0.0; /* stop vehicle for 30sec..40sec */ 


if (time>=4000) 
if (time<5000) 
speed=at (4000.0-time)/100.0; /* reverse motion, move back for 40sec .. 50sec */ 


if (time==5000) { 
speed=-at10.0; /* move back with constant velocity */ 


if (time>=6000) 
if (time<7000) 
speed=a* (time-7000.0)/100.0; 


if (time==7000){ 
mode=0; 
StopVME9325 () ; /* stop A/D-Board */ 
all0ffAndZero(); 

} 


/* compute required derivatives */ 

speedDot=(speed-motion.Speed) /DELTA_T; 
thetaDot=(theta-motion. Theta) /DELTA_T; 
omegaDot=(omega-motion.Omega) /DELTA_T; 


81 


726 
727 
728 
729 
730 
731 
732 
733 
734 
735 
736 
737 
738 
739 
740 
741 
742 
743 
744 
745 
746 
747 
748 
749 
750 
751 
752 
753 
754 
755 
756 
757 
758 
759 
760 
761 
762 
763 
764 
765 
766 
767 
768 
769 
770 
771 
772 
773 
774 
775 
776 
777 
778 
779 
780 
781 
782 
783 
784 
785 
786 
787 
788 
789 
790 
fal 
792 
793 
794 
795 
796 
797 
798 
(ey 
800 
801 


i) 


/* update the motion */ 
motion.Speed = speed; 
motion.Theta = theta; 
motion.Qmega = omega; 


/* update the vehicle configuration */ 

vehicle.heading = vehicle.heading + motion.Omega*DELTA_T; 

vehicle.coord.x = vehicle.coord.x + motion.Speed*DELTA_T * cos(motion. Theta) ; 
vehicle.coord.y = vehicle.coord.y + motion.Speed*DELTA_T * sin(motion. Theta) ; 


YR mr ee ee = * 
* drive motors (former in motor.c) “ 
BO a a a a a a a eee ee « / 


dd[(0]=speed/wheelRadius [0] *16615.776; 
dd(1]=speed/whee]Radius[1]*16615.776; 
dd [(2]=speed/wheel Radius [2] *16615.776; 
dd[3]=speed/wheel Radius [3]*16615.776; 


speedDigits[0]=(short) (0.0132421+*dd[0]-1.15119); /* set speed for wheel 1 */ 
speedDigits[1]=(short) (0.0132276*dd[1]-1.17617); /* set speed for wheel 2 */ 
speedDigits(2]=(short) (0.0132283*dd(2]+0.17110); /* set speed for wheel 3 */ 
speedDigits (3]=(short) (0.0132680*dd(2]+1.21652); /* set speed for wheel 4 */ 


/* set the speeds */ 
setServoSpeed(); 


return; 
/* end of leoMotion() */ 


void linearMotion2(void) 


! 


double vix, vly, v2, viyvixRatio,omega2,omega3, beta,ro,ro2,wheelAngleV; 
int ix,Speed_Digit ,Steer_Digit; 
short *servoOut; 


/* read all shaft encoders */ 
readNewEncoder(); 


/* compute the actual rates, velocities and angles */ 

for (ix=0; ix<4; ix++){ 
driveSpeed[ix]=driveDelta[ix]*CM_PER_DIGIT/DELTA_T; /* [cm/s] */ 
steerRate[ix]=steerDelta[ix] /DELTA_T; 
steerAngle[ix]=steerAngle[ix]+steerDelta[ix] *RAD_PER_DIGIT; 

} /* end of for ... */ 


/* initialize temporary variables */ 
speed=motion.Speed; 
theta=motion.Theta; 
omega=motion. Omega; 


[+ * 
* body motion (former in movement.c) x 
Ko See ee ee oe ee ee ee ee oe ee ee ee eee ree eee ne ee ee ee eee */ 

a=100.0; /* max acceleration [cm/sec”2] */ 


/* no acceleration for t<lsec */ 


if ((time>=100)&& (time<200) ) 
speed=0.005* (time-100)*(time-100); /* vehicle speed [cm/sec] (max is 50cm/sec */ 


if ((time>=300)k& (time<400) ) 
speed=800. 0+0.005*time* (time-800.0) ; 


if (time==400) { 
mode=0; 
stopVME9325 () ; /* stop A/D-Board */ 
all0OffAndZero(); 

} 


/* compute required derivatives */ 
speedDot=(speed-motion .Speed) /DELTA_T; 
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802 thetaDot=(theta-motion. Theta) /DELTA_T; 


803 omegaDot=(omega-motion. Omega) /DELTA_T; 

804 

805 /* update the motion */ ° 

806 motion.Speed = speed; 

807 motion. Theta = theta; 

808 motion.Qmega = omega; 

809 

810 /* update the vehicle configuration */ 

811 vehicle.heading = vehicle.heading + motion.Omega*DELTA_T; 

812 vehicle.coord.x = vehicle.coord.x + motion.Speed*DELTA_T * cos(motion. Theta) ; 
813 vehicle.coord.y = vehicle.coord.y + motion.Speed*DELTA_T * sin(motion.Theta) ; 
814 

815 / ie aaa Ae eae ce ne Lee ae * 
816 * drive motors (former in motor.c) * 
817 a a aa a a + / 
818 

819 dd[{0]=speed/wheelRadius [0] #16615.776; 

820 dd{1]=speed/wheelRadius [1] *16615.776; 

821 dd{2]=speed/wheelRadius [2] *16615.776; 

822 dd{3]=speed/wheelRadius [3] #16615.776; 

823 

824 

825 speedDigits [0]=(short) (0.0132421*dd[0]-1.15119) ; /* set speed for wheel 1 */ 
826 speedDigits[(1]=(short) (0.0132276*dd{1]-1.17617); /* set speed for wheel 2 */ 
827 speedDigits[(2]=(short) (0.0132283*dd[2]+0.17110); /* set speed for wheel 3 +/ 
828 speedDigits (3]=(short) (0.0132680*dd[2]+1.21652); /* set speed for wheel 4 */ 
829 

830 /* set the speeds */ 

831 setServoSpeed() ; 

832 

833 return; 

834 } /* end of leoMotion2() */ 

835 

836 

Bal [8 mmr or rn a rn rn rn rn ee nr enn on ne ence reson scrses=<<= * 
838 * setServoSpeed() * 
839+ * 
840 * Environment: GCC Compiler v2.7.2 * 
841 * Name: Thorsten Leonardy * 
842 * Last update: 10/27/97 . 
843 * Purpose: This function sets the speed as specified in global vars * 
844 * speedDigits and steerDigits to all servo motors. * 
845 * Called from: driver() in movement.c * 
B46 8 eee */ 
847 void setServoSpeed(void) 

848 { 

849 

850 short *servoOut=(unsigned short*) (VME9210+0x0082) ; /* Analog out */ 

851 

852 *servoOut++= -speedDigits [0] +16; /* set speed for driving wheel 1 */ 

853  *servoOut+t= speedDigits[1]+*16; /* set speed for driving wheel 2 */ 

854 ¥*servoDutt++= -speedDigits[2] +16; /* set speed for driving wheel 3 */ 

855 *servoOut++= speedDigits[3]*16; /* set speed for driving wheel 4 */ 

856 

857 *servo0ut++= steerDigits[0]*16; /* set speed for driving wheel 1 */ 

858 *servoOutt+t+t= steerDigits[1]*16; /* set speed for driving wheel 2 */ 

859 *servo0ut+t+= steerDigits[2]*16; /* set speed for driving wheel 3 */ 

860 *servoOutt+t+= steerDigits[3] #16; /* set speed for driving wheel 4 */ 

861 

862 return; 

863 } /* End of setServoSpeed */ 

864 

865 

866 

BET [8 nn ne tn rn * 
868 * clearEncoder (motors) * 
869 + * 
870 »* Environment: GCC Compiler v2.7.2 - 
871 * Last update: 03 November 1997 * 
872 * Name: Thorsten Leonardy * 
873 * Purpose: This function clears all shaft encoders. & 
874 * ~ 
875 * motors bit mask to select motors, eg. 0x042 selects motor 2 and 7 * 
876 *# to be cleared. * 
BT eS */ 


83 


878 void clearEncoder(unsigned char motors) 


879 
880 
881 
882 
883 
884 
885 
886 
887 
888 
889 
890 
891 
892 
893 
894 
895 
896 
897 
898 
899 
900 
901 
902 
903 
904 
905 
906 
907 
908 
909 
910 
911 
912 
913 
914 
915 
916 
917 
918 
919 
920 
921 
922 
923 
924 
925 
926 
927 
928 
929 
930 
931 
932 
933 
934 
935 
936 
937 
938 
939 
940 
941 
942 
943 
944 
945 
946 
947 
948 
949 
950 
951 
952 
953 


{ 


unsigned char *p=(unsigned char*)VMECTR1; 
ant ix; 


for (ix=0; ix<4; ix++,motors/=2) { 
if (motors & 0x01) *(p+3)=0x04; 


if (motors & 0x10) *(p+0x0103)=0x04; 


/* 


/* clear steering counter */ 


clear respective counter */ 


p=p+4; /* access next pointer + / 
} 
return; 

} /* end of clearEncoder */ 

[OR rr 9 ren * 
* align() * 
* Environment: GCC Compiler * 
* Last update: 07 August 1997 m * 
* Name: Thorsten Leonardy and Yutaka Kanayama * 
* Purpose: This function will align SHEPHERD’s wheels such that all - 
* will point in the forward direction. It utilizes the hall * 
* sensors for each of the four wheels. All wheels are being * 
* aligned simultaneously rather than successively. * 
* * 
Ro ween een en ow ww ww nw wr nn wr nn on orn on enn woe on oe ne ee eee ee en cee ee eres eee== * / 


void align(void) 


{ 


unsigned short *servoOut=(unsigned short*) (VME9210+0x008A) ; 


/* Analog out */ 


unsigned short *servoStatus=(unsigned short *) (VME9421+0x00ca); /* digital input */ 


unsigned int ‘*servoControl=(unsigned int *)VME2170; 


int ix; 
unsigned short bitMask,speed=0x0200; 


/* set steering speeds */ 
*servoQut=-speed; /* wheell -> 
*(servoOut+1)= speed; /* wheel? -> 
*(servoQ0ut+2)= speed; /* wheel3 -> 
*(servoOut+3)=-speed; /* wheel4 -> 


bitMask=0xf000,; 


while (bitMask) { 


if 


af 


if 


if 


} 
} 


( Ox8000 & *servoStatus ){ 
*servo0ut=0x0000; /* 
bitMask=bitMask & 0x7000; 


( 0x4000 & *servoStatus ){ 
*(servoOut+1)=0x0000; /* 
bitMask=bitMask & Oxb000; 


( 0x2000 & *servoStatus ){ 
*(servo0ut+2)=0x0000; /* 
bitMask=bitMask & 0xd000; 


( 0x1000 & *servoStatus ){ 
* (servo0ut+3)=0x0000; /* 
bitMask=bitMask & Oxe000; 


sioQut(0,"Aligned ...\n\r"); 


return; 
/* end of align */ 


set 


set 


set 


set 


rotate 
rotate 
rotate 
rotate 


speed=0 


speed=0 


speed=0 


speed=0 


/* Data Out */ 


CW +/ 

CCW */ 
CCW */ 
CW +/ 


for wheel 1 */ 


for wheel 2 */ 


for wheel 3 */ 


for wheel 4 */ 


* all servos on and set zero speed, [added 11/05/97, Leo] * 
we ee ee ne te 5 oe = = He eH - * / 
void allOnAndZero(void){ 
unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

short *servoOut=(unsigned short*) (VME9210+0x0082); /* Analog out driving wheell */ 
int 1x; 


for (ix=0; ix<8; ix++) *sergoQut++=0x0000; 


/* set zero speed */ 
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954 
955 
956 
957 
958 
959 
960 
961 
962 
963 
964 
965 
966 
967 
968 
969 
970 
971 
mes 
973 
974 
975 
976 
977 
978 
979 
980 
981 
982 
983 
984 
985 
986 
987 
988 
989 
990 
991 
992 
993 
994 
995 
996 
997 
998 
999 
1000 
1001 
1002 
1003 
1004 
1005 
1006 
1007 
1008 
1009 
1010 
1011 
1012 
1013 
1014 
1015 
1016 
1017 
1018 
1019 
1020 
1021 
1022 
1023 
1024 
1025 
1026 
1027 
1028 
1029 


*servoControl=0x00924924; /* turn on all motors */ 


return; 
} /* end of allOnAndZero */ 


[OR rr rn en eee * 
* all servos off and set zero speed, [added 11/05/97, Leo] * 
Bo 9 nn eee no-e */ 


void all0ffAndZero(void) { 
unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 
short *servoOut=(unsigned short*) (VME9210+0x0082); /* Analog out driving wheell */ 
int’ 1x; 


for (ix=0; ix<8; ix++) *servo0ut++=0x0000; /* set zero speed */ 
*servoControl=0x00000000; /* turn on all motors */ 


return; 
} /* end of all0ffAndZero */ 


[Rr ee * 
* Set all driving motors to specific speed * 
RO ern nn ee en re enn wn oe ne eee ee ee eee ee « / 


void allDrive(short digit){ 
unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 
short *servo0ut=(unsigned short*) (VME9210+0x0082); /* Analog out driving wheell +*/ 
int 1x; 


for (ix=0; ix<4; ix++) *servoOutt++=digit; /* set zero speed */ 
*servoControl=0x00000924; /* turn on driving motors */ 


return; 
} /* end of allDrive */ 


(meee tee ee en * 
* Set all steering motors to specific speed * 
Ko teem eee ee en eee oe = / 

void allSteer(short digit) 

{ 


unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

short *servoQut=(unsigned short*) (VME9210+0x008A); /* Analog out steering wheeli +/ 
int 1x; 

for (ix=0; ix<4; ix++) *servoOut++=digit; /* set zero speed */ 


*servoContro1=0x00924000; /* turn on steering motors */ 


return; 
} /* end of allSteer */ 


[comer a on a ee eee * 
* switches all motors off [added 11/05/97, Leo] * 
Ko ee nn to ne oe oo ee a/ 


void allMotorsOff (void) { 
unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 


*servoControl=0x00000000; /* turn off all motors */ 


return; 
} /* end of allMotorsOff +*/ 


[Rm rr rt rrr errr rere * 
* switches all motors on [added 11/05/97, Leo] * 
Ko ewer er ee eee en ee n-ne ee = * / 


void allMotorsOn(void) { 
unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 


*servoControl=0x00924924; /* turn on all motors */ 


return; 
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1030 } /* end of allMotorsOn */ 


1031 

1032 
a a a aaa * 
1034 * driveTest() . 
1035 * * 
1036 »* Environment: GCC Compiler v2.7.2 * 
1037 »* Last update: 29 October 1997 * 
1038 »* Name: Thorsten Leonardy * 
1039 * Purpose: This function computes the actual servo data for all * 
1040 —* driving motors. * 
1041 * Called from: user() upon keyboard interaction (type ’d’) + 
1042 eee a a eee Se ee neon n—— */ 
1043. void driveTest() 

1044 { 

1045 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 
1046 unsigned short *servoOut=(unsigned short*) (VME9210+0x008A) ; /* Analog out */ 
1047 unsigned short *servoStatus=(unsigned short *) (VME9421+0x00ca); /* digital input */ 
1048 unsigned short bitMask=0x8000; /* access bit 15 for align wheel 1 */ 
1049 unsigned char *p; 

1050 unsigned int wheelSelect; 

1051 int ix; 

1052 

1053 *servoControl=0x00000000; /* disable (turn off) all wheels */ 
1054 

1055 servoOut=(unsigned short*) (VME9210+0x0082) ; /* Analog out for drive wheel 1*/ 
1056 whee1Select=0x00000004; /* select servo for driving wheel 1 */ 
1057 

1058 p=(unsigned char*) VMECTR1; 

1059 

1060 for (ix=0; ix<4; ix++) { 

1061 

1062 *xservoOut=testSpeed; /* set output value for servo first «/ 
1063 *servoControl=wheelSelect; /* turn on selected servo motor * / 
1064 

1065 sioOut(0,"Press ’.’ to start recording time\n\r"); 

1066 

1067 while (key!=’.’) ; /* wait until user starts */ 

1068 

1069 * (p+3)=0x04; /* clear counter for driving wheel ix */ 
1070 

1071 readOneEncoder (ix, (int *)&driveCountPrevious[ix]); /* update encoder */ 
1072 readOneEncoder(ix,(int *)&steerCountPrevious[ix]); /* update encoder */ 
1073 

1074 timeF orTurn(ix]=intCounter; /* store time (start observing) */ 

1075 

1076 sioOut(0,"Press ’,’ to stop recording time\n\r") ; 

1077 

1078 while (key!=’,’) ; /* wait until user stops the process */ 
1079 

1080 timeForTurn[ix]=intCounter-timeForTurn [ix] ; 

1081 

1082 *servoO0ut++=0x0000; /* stop wheel */ 

1083 

1084 readOneEncoder (ix, (int *)&driveCount[ix]); /* update encoder */ 

1085 readOneEncoder (ix, (int *)&steerCount [ix]); /* update encoder */ 

1086 

1087 driveDelta[ix]=(driveCount [ix] -driveCountPrevious[ix] )/256; 

1088 steerDelta[ix]=(steerCount [ix]-steerCountPrevious[ix]) /256; 

1089 

1090 wheelSelect= wheelSelect<<3; /* select next servo (motor) x / 
1091 

1092 Vy 

1093 

1094 *servoControl=0x00000000; /* disable (turn off) all wheels */ 
1095 

1096 return; 

1097 } /* end of driveTest */ 

109S 

1099 

LL00 [Ferner rere en on nr nn ee sean ee en ea een enn seceasssessesens—-— a 
1101 * velocityTest() au 
102 # * 
1103 * Environment: GCC Compiler v2.7.2 * 
1104 * Last update: 07 November 1997 * 
1105 * Name: Thorsten Leonardy * 
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1106 
1107 
1108 
1109 
1110 
1111 
Ev12 
1113 
1114 
1115 
1116 
1117 
1118 
1119 
1120 
1121 
1122 
1123 
1124 
1125 
1126 
127 
1128 
1129 
1130 
1131 
1132 
1133 
1134 
1135 
1136 
1137 
1138 
1139 
1140 
1141 
1142 
1143 
1144 
1145 
1146 
1147 
1148 
1149 
1150 
1151 
1152 
1153 
1154 
1155 
1156 
1157 
1158 
1159 
1160 
1161 
1162 
1163 
1164 
1165 
1166 
1167 
1168 
1169 
1170 
1171 
1172 
1173 
1174 
1175 
1176 
1177 
1178 
1179 
1180 


Purpose: This function obtaines the velocity versus digit curve. . 
Drive servos are given different velociies (digit) every * 
two seconds. The first second is to obtain steady state, the* 
second second will record the shaft encoder difference, thus* 


giving rise to a encoder reading versus velocity curve. * 

The commanded velocity goes from 500 .. -510 at present. * 

* 

Called from: user() upon keyboard interaction (type ’v’) * 
a rr rr rn nnn oe - += */ 


void velocityTest (void) 


{ 


unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 
short *servoQut=(unsigned short*) (VME9210+0x0082);  /* Analog out driving wheel1 */ 


short speed,digit; 


speed=500; 
digit=speed*16; 


leoData=(int *)0x00100000; /* start data storage */ 
sioQut(0,"velocityTest\n\r") ; 

align() ; 

all0ffAndZero() ; 


*servoControl1=0100000924 ; /* turn on driving motors */ 


readNewEncoder (); 
time=0; /* this will be altered by timer interrupt */ 


/* set new driving values */ 


*servo0ut+t+=-digit; /* set speed for wheel 1 */ 
*servoQutt+t+= digit; /* set speed for wheel 2 */ 
*servo0utt++=-digit; /* set speed for wheel 3 */ 


*servoQut++= digit; /* set speed for wheel 4 */ 
while (speed>-510) { 
servoQut=(short *) (VME9210+0x0082) ; 


/* set new driving values */ 


*servoQut++=-digit; /* set speed for wheel 1 */ 
*servoOut+t+= digit; /* set speed for wheel 2 */ 
*servoQut++=-digit; /* set speed for wheel 3 */ 


*servoQut++= digit; /* set speed for wheel 4 */ 


speed=speed-10; 
digit=speed*16;  /* shift nibble left */ 
time=0; 


/* wait a second for motors to settle */ 
while(time<100) ; 


readNewEncoder() ; 


/* record for a second */ 
while (time<200) ; 


readNewEncoder(); 


/* store the counter data for previous speed */ 
*leoDatatt+=steerDelta[0] ; 
*leoDatat++=steerDelta([1] ; 
*leoDatat++=steerDelta[2] ; 
*leoDatat+=steerDelta([3]; 
*leoDatat+=driveDelta[0]; 
*leoData++=driveDelta[1]; 
*leoDatat+=driveDelta([2]; 
*leoData++=driveDelta [3] ; 

} 


all0ffAndZero(); 


return; 


1181 } /* end of velocityTest */ 
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1182 


1183 

Bd) See eS Se ea a ee ee ea ne * 
1185 * circumferenceTest() * 
1186 * * 
1187 * Environment: GCC Compiler v2.7.2 * 
1188 * Last update: 07 November 1997 * 
1189 * Name: Thorsten Leonardy * 
1190 * Purpose: This function drives the vehicle in a straight line and * 
591 60 stores the difference for all shaft encoders for a given * 
1192 _* observation time. If the distance travelled is being * 
1193 * measured, one can obtain the relation between shaft encoder * 
1194 * readings and wheel diameter. * 
1195 * Called from: user() upon keyboard interaction (type ’c’) * 
ee ee a mee */ 
1197 void circumferenceTest (void) 

1198 { 

1199 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

1200 short *servoOut=(unsigned short*) (VME9210+0x0082); /* Analog out driving wheell */ 
1201 

1202 short speed,digit; 

1203 

1204 speed=300; 

1205 digit=speed*16; 

1206 

1207 leoData=(int *)0x00100000; /* start data storage */ 

1208 

1209 sioOut(0,"circumferenceTest ()\n\r") ; 

1210 

1211 align(); 

1212 all0OffAndZero(); 

1213 

1214 *servoControl=0x00000924 ; /* turn on driving motors */ 

1215 

1216 /* determine the digits to command based on linea4r relationship obtained * 
1217 * in velocityTest for each wheel individually. */ 
1218 

1219 /* assume for one second, that driveDelta=10000 */ 

1220 

1221 

1222 /* set new driving values for driveDelta approx 10000 over 1 sec */ 

1223 *servoDut++= (short) (-16*(0.0132421*ddc-1.15119));  /* set speed for wheel 1 */ 
1224 *servoQut++=(short) ( 16*(0.0132276*ddc-1.17617)); /* set speed for wheel 2 */ 
1225 *servoQut++= (short) (-16*(0.0132283*ddc+0.17110)); /* set speed for wheel 3 */ 
1226 *servoQut++=(short) ( 16*(0.0132680*ddc+1.21652)); /* set speed for wheel 4 */ 
1227 

1228 time=0; /* this will be altered by timer interrupt */ 

1229 readNewEncoder(); 

1230 

1231 while (time<tc) ; /* wait 2 sec */ 

1232 

1233 readNewEncoder(); 

1234 

1235 all0ffAndZero(); 

1236 

1237 return; 

1238 } /* end of circumferenceTest */ 

1239 

1240 

1241 

1242 [8 mmm nn ren nn nnn nnn nn nnn nn een e nn en ene n een ee seen nceeeseeen es seenne= * 
1243 * steerTest() * 
1244 * 

1245 * Environment: GCC Compiler v2.7.2 * 
1246 * Last update: 29 October 1997 1c 
1247 * Name: Thorsten Leonardy * 
1248 * Purpose: This function computes the actual servo readings for all * 
iz490Ci¥ steering motors. * 
1250 * Called from: user() upon keyboard interaction (type ’w’) * 
Sa ee a */ 
1252 void steerTest() 

1253 { 

1254 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 
1255 unsigned short *servoDut=(unsigned short *) (VME9210+0x008A) ; /* Analog out */ 
1256 unsigned short *servoStatus=(unsigned short *) (VME9421+0x00ca); /* digital input */ 
1257 unsigned char *p; 
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1258 
1259 
1260 
1261 
1262 
1263 
1264 
1265 
1266 
1267 
1268 
1269 
1270 
1271 
1272 
1273 
1274 
1275 
1276 
1277 
1278 
1279 
1280 
1281 
1282 
1283 
1284 
1285 
1286 
1287 
1288 
1289 
1290 
1291 
1292 
1293 
1294 
1295 
1296 
ot 
1298 
1299 
1300 
1301 
1302 
1303 
1304 
1305 
1306 
1307 
1308 
1309 
1310 
1311 
1312 
1313 
1314 
1315 
1316 
1317 
1318 
1319 
1320 
1321 
1322 
1323 
1324 
1325 
1326 
1327 
1328 
1329 
1330 
1331 
1332 
1333 


unsigned short bitMask=0x8000; /* access bit 15 for align wheel 1 */ 
unsigned int wheelSelect=0x00004000; /* select servo for turning wheel 1 */ 
int ix,turns,a; 


/* align wheels */ 
align(); 


/* clear all driving and steering motor counters and the variables */ 
clearEncoder (Oxff); 


servoQut=(unsigned short*) (VME9210+0x008A); /* Analog out for steering wheel 1 */ 


bitMask=0x8000; /* access bit 15 for align wheel 1 */ 
wheelSelect=0x00004000; /* select servo for turning wheel 1 */ 
readNewEncoder(); /* read all encoders */ 


for (ix=0; ix<4; ix++) { 


turns=0; 
*servo0ut=testSpeed; /* set output value for servo first * / 
*servoControl=wheelSelect; /* turn on selected servo motor * / 


/* turn wheels for a total of 10 turns */ 


do { 
while(! (bitMask&*servoStatus)); /* wait until wheel aligned */ 
while(bitMask&*servoStatus) ; /* wait until wheel progressed */ 
turns++; /* one turn completed */ 
if (turns==1) 


timeForTurn [ix]=intCounter; /* store time (start observing) */ 
if (turns==9){ 
timeForTurn [ix])=(intCounter-timeForTurn[ix])/8; /* stop timer */ 


*servo0ut++=0x0800; /* speed for final turn */ 
} 
}while (turns<10); 
wheelSelect= wheelSelect<<3; /* select next servo (motor) * / 
bitMask = bitMask >> 1; /* select ner xt status align bit * / 
} 
*servoControl=0x00000000; /* disable (turn off) all wheels * / 


readNewEncoder(); 
for (ix=0; ix<4; ix++) radPerDigit [ix]=2.0*P1I*10.0/(double)steerDelta[ix]; 


return; 


} /* end of steerTest */ 


/* 


e* ee te & H F 


+ 


ee ae a oe PR a * 
stopTest() * 
* 
Environment: GCC Compiler v2.7.2 * 
Last update: 03 November 1997 = 
Name: Thorsten Leonardy * 
Purpose: This function computes the actual servo readings for all * 
steering motors while the motor speeds are set to zero. * 
Called from: user() upon keyboard interaction (type ’s’) we 
were nn on nn on oo oo a nn ee er ene ene */ 


void stopTest() 


“t 


sioQut(0,"Aligning Wheels ...\n\r"); 
align(); /* align wheels */ 


/* clear all driving and steering motor counters and the variables */ 
clearEncoder (Oxff); 


readNewEncoder(); 


allOnAndZero(); 

time=0; 

sioOut(0,"Please Wait a minute ...\n\r"); 
while (time<6000) ; /* wait a minute */ 


all0OffAndZero(); 
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1334 
1335 
1336 
1337 
1338 
1339 
1340 
1341 
1342 
1343 
1344 
1345 
1346 


sioOut(0,"Done\n\r") ; 
readNewEncoder(); 


return; 
} /* end of stopTest */ 


[EERE RAAAE EEE ERE RRS EEE EERE EEE EKER EERE ERE EEE EERE EEE EERE EEE EEE EEE EE 


End of motor.c 
SERIE EIOIOI IOI IOIOIOI III IOC CR iO OGIO IO i iO OI io GIO Iai aioio aiuto ior iio iit a toe / 
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APPENDIX D: SHEPHERD PRIMER 


This appendix provides essential data and procedures which lead to the findings of the 
motion parameters that are required to operate SHEPHERD properly. Boxed text will refer to a 
segment of software code or a command sequence for use in the TAURUS Debugger environment. 
The focus is on the use of the TUARUS Debugger since this provides a quick way to determine most 


of the operating parameters. 


il. MAIN OPERATING PARAMETERS AND CONVERSION FACTORS 


It is sometimes tedious to gather the meat for operating a system. This section strives to 


provide most of the operating parameters pertaining to the use of SHEPHERD in tabulated form. 


Wheel Radius 0.189 m 

max. Tire pressure 49.8 psi 

Drive Encoder (all Wheels) 2 7 radians = 360 * 290 counts 
1 m = 87914 counts 
ieecount = Ula 7am 


Wheel 1 digit = 187.20 v [cm/sec] - 26.4 
Wheel 2 digit = 187.04 v [cm/sec] - 26.4 
Wheel 3 digit = 186.88 v [cm/sec] - 4.8 

Wheel 4 digit = 187.20 v [cm/sec] + 8.8 


Steer Encoder (all Wheels) 2 7 radians = 360 * 256 counts 
1 degree = 256 counts 


Table 4.1: Shepherd Operating Parameters in a Nutshell 
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2. RESET AND READ SHAFT ENCODERS 


To find out how the servo readings relate to either the steering and/or the driving, use 
the following debugger sequence which resets the servo counter for one wheel, drives the wheel and 
reads the servo counter after steering is done. The same procedures would apply for use with the 


remaining servo motors. 


Teurus_Bug>ms ffff6idb 04 & clear servo counter for steering wheel 3 
Taurus_Bug>ms f£fff04Be OBOO & set velocity for steering wheel 3 
Teurus_Bug>ms ffffPTOO 00100000 & turn on motor for steering wheel 3 
& ... efter a certain number of revolutions ... 
turn off motor for steering wheel 3 
select control for motor 7 (steer wheel 3) 


Teurus_Bug>ms ffffff00 00000000 & 

Taurus_Bug>ms ffff610b 03 & 

Taurus_Bug>ms f£fff610b 01 & 

Taurus_Bug>ad ffff6i09:i;b & read least significant byts of 24bit counter 

FFFF6i09 03 & the result 

Taurus_Bug>md ffff6i09:1;b & reed next byte ... 

FFFF6109 C6 & ... the result 

Teurue_Bug>md ffff6i09:1;b & reed most significant byte .. 

FFFF6109 FB & ... the result 

Taurus_6ug> & the complete counter value in this case is 
& Oxfbc6d3 sign-extended (e.g. -2767B1) 


3. UP- AND DOWNLOADING DATA FROM TAURUS BOARD 


At this time, there is no straight forward routine for data up- and downloading available. 
Hence, the up- and downloading of data such as waypoints, ... is very tedious. The only way, data 
can be transferred from or to the TAURUS main memory is via the TAURUSBug options ‘du’ for 
downloading data to the Laptop and ‘lo’. However, data would be made available only in form of 


the Motorola 5-Record format. 
To download data from the TAURSU main memory to the Laptop, the Laptop must capture 
the script sent to the screen to a file (option ”T” ext ”C” apture on the menu bar). In a second step, 


output the data to the screen using the folowing command: 


Taurus_Bug>du0 100000 1000ff ‘This is a dump to the screen’ 
Effective address: 00100000 

Effsctive addrass: 001000ff 
$01F00006468697320697 3206 1206476607020746F20746B66 207 3637 266656EFi 
$2141000001234123412341234123412341234i234AB 
$214100010123412341234123412341234123412349B 
$2141000201234123412341234 12341234123412346B 
$214 100030123412341234123412341234123412347B 
$21410004012341234123412341234123412341234BB 
$214100060 12341234123412341234 12341234123456 
$21410006012341234 12341234 12341234123412344B 
$2141000701234123412341234123412341234123436 
$214100080123412341234123412341234123412342B 
$214100090 12341234 123412341234123412341234iB 
$2 141000A0123412341234123412341234123412340B 
$2141000B01234 1234 123412341234 12341234i1234FB 
$2141000C012341234123412341234123412341234EB 
$214 100000123412341234123412341234123412340B 
$2141000E0i12341234123412341234123412341234CB 
$2141000F0 12341234123412341234123412341234BB 
$9030000FC 

Tsurus ._Bug> 
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As can be seen above, the data from memory location 0x100000 to 0x1000ff will be output 
to the screen and thus captured in the ascii file specified. However, the data will be in the Motorola 
S-Record format and a parsing program needs to extract the pure data. The parsing program 
however, needs to know the datatype of the data given to extract the correct information. E.g., 
extracting data of datatype ‘integer’ would require a different parsing routine. 

As far as the uploading of data is concerned, the datafile must be transferred in the same 


manner as the SRK program, with the ‘LO’ option and described by [1]. 


4. INTERRUPTS 


This section describes briefly what type of interrupts are enabled on SHEPHERD. 


a. Timer Interrupt 


Every 10 ms, a timer interrupt is issued by the on board timing circuit. The interrupt 
handling routine ’TimerHandler’ does the following: 
1. increments counter ’intCounter’ 
(which may be needed for timing purposes) 
2. initiate (software trigger) a block conversion for the A/D-Board AVME9325-5 


3. call function driver’ in file ‘movement.c’ to execute/handle motion control part 


The interrupt is routed through the Interrupt steering mechanism (ISM) to the VIC068 and 
from there to the 68040 processor in the following way: 













LIRQ-3 IRQ-3 
Dewan Lewel22 ISM — VIC068 ia 68040 


IACK-3 


b. A/D-Board Interrupt 


Every 10 ms, the timer circuit initiates the start of a block conversion on the A/D-Board. 


Once this conversion is complete, the A/D-Board AVME9325-5 issues an interrupt to indicate that 
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the conversion is complete and data is available to be read from its dual port RAM. The interrupt 
handler ‘handler VME9325()’ then subsequently calls ’analyzeData’ to further analyze/process 
the data. The interrupt vector number is provided by the Board and set to be 0x0056 which relates 
to the location of the address for interrupt handling routine at 0x0158 in the interrupt vector table. 

As opposed to on-board interrupts, the interrupt from the A/D-Converter VME board is 
routed directly through the VIC068 to the 68040 processor: 


VMEBus 





VIC068 


CG Keyboard Interrupt 


The overarching framework for user interaction is provided by the routine ’user()’ in file 
-user.c’. Each time, the keyboard is pressed, an interrupt is issued by the 68C681 on board serial 
circuit to the 68040 through the ISM and VICO68. The ascii code for the key pressed is then be 
stored in the variable inPortA and further analyzed by the routine ‘user()’ in file ‘user.c’. The mode 
flags set in this function will be further processed by functions called during the motion control cycle 
following each 10ms timer interval. For this interrupt, the interrupt vector number is provided by 
the DUART and set to be 0x0060 thus giving rise to the location of the interrupt handling routine 
inPortAHandler at 0x0180 in the interrupt vector table. 


68C681 











Level 8 





LIRQ-1 IRQ-1 


IACK-1 


5. REPRESENTATION OF DOUBLE VARIABLES 
According to the M68040 users manual, any double-precision variable is stored in memory 


as an 8 byte data value in the following form 


Since the representation is normalized with the leading (implicit) bit always one we find the relation 
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Bit 63 = s = sign bit (l=negative number) 


Bit 62..52 = e = 11 bit exponent in the range 0x000 ... Ox7ff 
Bit 51..0 = f = 52 bit (13 nibbles) binary decimal (mantissa) 
in the range OxO000000000000 ... Oxfffffffffffff 


to the real number representation x by 


r= (-1)° ge—Ox3tt (1 ate d) 


with d = f-2-°* . As an example, to display the double variable stored in memory location 0x306e8 


we issue the following TAURUSbug commands 


Taurus_Bug>md 306e8:1i;d 


OOO306E8 1_3F1_1DF44179E4364 





The result is conveniently displayed by the monitor such that the elements can be easily identified: 


s=1, e=0x03f1, f=0x1df£44179e4364. Hence, the real number is 


= 0x01df£44179e4364 
0x10000000000000 


6. HOW TO RUN SHEPHERD’S WHEELS 


Three VME boards account for operating of the wheels, both in steering and driving. These 


boards are accessible via the VME Bus Port connector P1 and they are: 













Board 
VME 9210 
VME 2170 
VME 9421 


GCC Access 
short 

unsigned int 
unsigned short 


Function 
Analog Output to servos (velocity) 
Servo Control (on/off) | 
Servo Status 











Shepherd is equipped with a total of eight servo motors: four wheels with driving and 
turning capability. The setup and software configuration is depicted in Figure (1). In order to 


operate each one of the motors one has to perform the following steps: 


1. Select the angular velocity for the motor by writing a signed short value (16 Bit) to the 
respective channel (see Figure 1 for the channel assignments) on the VME9210 board (analog 
Output). E.g. to turn wheel 3 (rear right) one would write 


* (ff£££048e)=(short) velocity; 
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where a positive velocity corresponds to the spin direction as indicated by the arrow in Fig. 
(1). The well known Right-Hand rule applies for determining the direction of spin. 


2. Switch the motor on/off by writing the respective mask to VME2170 at Oxffffff00. Refer 
to Fig. (1) for the mask assignment. E.g. to drive wheel 2 (front left) and turn wheel 4 
(rear left) simultaneously, one would issue the command 


* (Oxffffff00)=(unsigned int )0x00800020 





Any combination is allowed, i.e. mask 0x00900000 would turn wheels 3 and 4. Make sure 
you have set the angular velocities for the wheels you are going to run as outlined in step 1 
above! 





A word of Caution: for driving wheels 1 (front right) and 3 (rear right) forward, negative values must be 


written to the VME9210 Board as outlined in step 1. 





drive Oxfffr0484 0x00000020 
steer Oxffffo48e 0x00020000 


drive Oxffff0482 0x00000004 
steer Oxffff048a 0x00004000 


Shepherd 


Vieel 3 


drive Oxffff0488 0x00000800 drive Oxffft0486 0x00000100 
steer Oxtftft0490 0x00800000 steer Oxffff048e 0x00100000 


Switchbox (aft) G 


Figure 4.1: Wheel Assignment and Servo Register Addressing (Arrows and Dots at each wheel 
indicate the rotation of the respective servos if controlled with positive values. 





Ga 
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